Skip to content

Commit

Permalink
Unuse simulated time
Browse files Browse the repository at this point in the history
  • Loading branch information
at-wat committed Mar 22, 2024
1 parent 6382b0d commit a9ffd39
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 17 deletions.
15 changes: 0 additions & 15 deletions test/src/joint_trajectory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,10 @@
TEST(JointTrajectory, CommandValidation)
{
ros::WallDuration wait(0.05);
ros::Duration clock_step(0.05);

ros::NodeHandle nh;
ros::Publisher pub_cmd =
nh.advertise<trajectory_msgs::JointTrajectory>("joint_trajectory", 1, true);
ros::Publisher pub_clock =
nh.advertise<rosgraph_msgs::Clock>("clock", 100);

sensor_msgs::JointState::ConstPtr joint_states;
const boost::function<void(const sensor_msgs::JointState::ConstPtr&)> cb_joint =
Expand All @@ -55,15 +52,9 @@ TEST(JointTrajectory, CommandValidation)
ros::Subscriber sub_joint_states =
nh.subscribe("joint_states", 100, cb_joint);

rosgraph_msgs::Clock clock;
clock.clock.fromNSec(ros::WallTime::now().toNSec());
pub_clock.publish(clock);

// Wait until ypspur_ros
for (int i = 0; i < 20 * 30; ++i)
{
clock.clock += clock_step;
pub_clock.publish(clock);
wait.sleep();
ros::spinOnce();
if (joint_states)
Expand All @@ -87,8 +78,6 @@ TEST(JointTrajectory, CommandValidation)

for (int i = 0; i < 50; ++i)
{
clock.clock += clock_step;
pub_clock.publish(clock);
wait.sleep();
ros::spinOnce();
}
Expand All @@ -109,8 +98,6 @@ TEST(JointTrajectory, CommandValidation)
wait.sleep();
for (int i = 0; i < 50; ++i)
{
clock.clock += clock_step;
pub_clock.publish(clock);
wait.sleep();
ros::spinOnce();
}
Expand All @@ -125,8 +112,6 @@ TEST(JointTrajectory, CommandValidation)
wait.sleep();
for (int i = 0; i < 50; ++i)
{
clock.clock += clock_step;
pub_clock.publish(clock);
wait.sleep();
ros::spinOnce();
}
Expand Down
2 changes: 0 additions & 2 deletions test/test/joint_trajectory.test
Original file line number Diff line number Diff line change
@@ -1,7 +1,5 @@
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="true" />

<test test-name="test_joint_trajectory" pkg="ypspur_ros" type="test_joint_trajectory" />

<node pkg="ypspur_ros" type="ypspur_ros" name="ypspur_ros">
Expand Down

0 comments on commit a9ffd39

Please sign in to comment.