From 9bc9fe7b321c46955109fd7315c8a3da66ae8faf Mon Sep 17 00:00:00 2001 From: Joachim Schleicher Date: Fri, 13 Oct 2017 17:14:41 +0200 Subject: [PATCH] add test case that fails without e53b364 See pull request for fix/jerk_on_stop for detailed description. This commit adds a delay subscriber to rrbot to emulate a hardware controller with dead time. The jump backwards in the actual position is observed in joint_trajectory_controller_test by extending the stateCB. The new testCase fails, if the velocity of the actual position is negative although motion should only be forwards. --- .../test/joint_trajectory_controller_test.cpp | 86 +++++++++++++++++++ joint_trajectory_controller/test/rrbot.cpp | 32 ++++++- .../test/rrbot_controllers.yaml | 2 + 3 files changed, 116 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index f6c37100f..0abd07030 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -39,6 +40,7 @@ #include #include +#include #include #include #include @@ -62,6 +64,9 @@ class JointTrajectoryControllerTest : public ::testing::Test joint_names[0] = "joint1"; joint_names[1] = "joint2"; + controller_min_actual_velocity.resize(n_joints); + controller_max_actual_velocity.resize(n_joints); + trajectory_msgs::JointTrajectoryPoint point; point.positions.resize(n_joints, 0.0); point.velocities.resize(n_joints, 0.0); @@ -96,6 +101,9 @@ class JointTrajectoryControllerTest : public ::testing::Test // Smoothing publisher (determines how well the robot follows a trajectory) smoothing_pub = ros::NodeHandle().advertise("smoothing", 1); + // Delay publisher (allows to simulate a delay of one cycle in the hardware interface) + delay_pub = ros::NodeHandle().advertise("delay", 1); + // Trajectory publisher traj_pub = nh.advertise("command", 1); @@ -141,6 +149,7 @@ class JointTrajectoryControllerTest : public ::testing::Test ros::Duration long_timeout; ros::Publisher smoothing_pub; + ros::Publisher delay_pub; ros::Publisher traj_pub; ros::Subscriber state_sub; ros::ServiceClient query_state_service; @@ -149,11 +158,20 @@ class JointTrajectoryControllerTest : public ::testing::Test StateConstPtr controller_state; + std::vector controller_min_actual_velocity; + std::vector controller_max_actual_velocity; void stateCB(const StateConstPtr& state) { boost::mutex::scoped_lock lock(mutex); controller_state = state; + + std::transform(controller_min_actual_velocity.begin(), controller_min_actual_velocity.end(), + state->actual.velocities.begin(), controller_min_actual_velocity.begin(), + std::min); + std::transform(controller_max_actual_velocity.begin(), controller_max_actual_velocity.end(), + state->actual.velocities.begin(), controller_max_actual_velocity.begin(), + std::max); } StateConstPtr getState() @@ -177,6 +195,28 @@ class JointTrajectoryControllerTest : public ::testing::Test return init_ok; } + std::vector getMinActualVelocity() + { + boost::mutex::scoped_lock lock(mutex); + return controller_min_actual_velocity; + } + + std::vector getMaxActualVelocity() + { + boost::mutex::scoped_lock lock(mutex); + return controller_max_actual_velocity; + } + + void resetActualVelocityObserver() + { + boost::mutex::scoped_lock lock(mutex); + for(size_t i = 0; i < controller_min_actual_velocity.size(); ++i) + { + controller_min_actual_velocity[i] = std::numeric_limits::infinity(); + controller_max_actual_velocity[i] = -std::numeric_limits::infinity(); + } + } + static bool waitForState(const ActionClientPtr& action_client, const actionlib::SimpleClientGoalState& state, const ros::Duration& timeout) @@ -731,6 +771,52 @@ TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTraj) } } +TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTrajWithDelay) +{ + ASSERT_TRUE(action_client->waitForServer(long_timeout)); + + // Go to home configuration, we need known initial conditions + traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_home_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + + // Make robot respond with a delay + { + std_msgs::Bool delay; + delay.data = true; + delay_pub.publish(delay); + ros::Duration(0.5).sleep(); + } + resetActualVelocityObserver(); + + // Send trajectory + traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately + action_client->sendGoal(traj_goal); + EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout)); + + ros::Duration wait_duration = traj.points.front().time_from_start * 0.5; + wait_duration.sleep(); // Wait until half of first segment + + ActionGoal empty_goal; + empty_goal.trajectory.joint_names = joint_names; + action_client->sendGoal(empty_goal); + ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout)); + + std::vector minVelocity = getMinActualVelocity(); + for(size_t i = 0; i < minVelocity.size(); ++i) + { + EXPECT_GE(minVelocity[i], 0.); + } + + // Restore perfect control + { + std_msgs::Bool delay; + delay.data = false; + delay_pub.publish(delay); + ros::Duration(0.5).sleep(); + } +} + TEST_F(JointTrajectoryControllerTest, emptyActionCancelsTopicTraj) { ASSERT_TRUE(initState()); diff --git a/joint_trajectory_controller/test/rrbot.cpp b/joint_trajectory_controller/test/rrbot.cpp index f304f9a71..29919ba8c 100644 --- a/joint_trajectory_controller/test/rrbot.cpp +++ b/joint_trajectory_controller/test/rrbot.cpp @@ -30,6 +30,7 @@ // ROS #include #include +#include // ros_control #include @@ -48,7 +49,9 @@ class RRbot : public hardware_interface::RobotHW vel_[0] = 0.0; vel_[1] = 0.0; eff_[0] = 0.0; eff_[1] = 0.0; pos_cmd_[0] = 0.0; pos_cmd_[1] = 0.0; + pos_lastcmd_[0] = 0.0; pos_lastcmd_[1] = 0.0; vel_cmd_[0] = 0.0; vel_cmd_[1] = 0.0; + vel_lastcmd_[0] = 0.0; vel_lastcmd_[1] = 0.0; // Connect and register the joint state interface hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]); @@ -80,6 +83,10 @@ class RRbot : public hardware_interface::RobotHW // Smoothing subscriber smoothing_sub_ = ros::NodeHandle().subscribe("smoothing", 1, &RRbot::smoothingCB, this); smoothing_.initRT(0.0); + + // Delay subscriber: delay==0 yields direct control, one cycle delay otherwise + delay_sub_ = ros::NodeHandle().subscribe("delay", 1, &RRbot::delayCB, this); + delay_.initRT(false); } ros::Time getTime() const {return ros::Time::now();} @@ -90,14 +97,26 @@ class RRbot : public hardware_interface::RobotHW void write() { const double smoothing = *(smoothing_.readFromRT()); + const bool delay = *(delay_.readFromRT()); + for (unsigned int i = 0; i < 2; ++i) { - if(active_interface_[i] == "hardware_interface::PositionJointInterface") + if(delay != 0) + { + std::swap(pos_cmd_[i], pos_lastcmd_[i]); + std::swap(vel_cmd_[i], vel_lastcmd_[i]); + } + else { - vel_[i] = (pos_cmd_[i] - pos_[i]) / getPeriod().toSec(); + pos_lastcmd_[i] = pos_cmd_[i]; + vel_lastcmd_[i] = vel_cmd_[i]; + } + if(active_interface_[i] == "hardware_interface::PositionJointInterface") + { const double next_pos = smoothing * pos_[i] + (1.0 - smoothing) * pos_cmd_[i]; - pos_[i] = next_pos; + vel_[i] = (next_pos - pos_[i]) / getPeriod().toSec(); + pos_[i] = next_pos; } else if(active_interface_[i] == "hardware_interface::VelocityJointInterface") { @@ -146,6 +165,8 @@ class RRbot : public hardware_interface::RobotHW hardware_interface::VelocityJointInterface jnt_vel_interface_; double pos_cmd_[2]; double vel_cmd_[2]; + double pos_lastcmd_[2]; + double vel_lastcmd_[2]; double pos_[2]; double vel_[2]; double eff_[2]; @@ -155,8 +176,11 @@ class RRbot : public hardware_interface::RobotHW realtime_tools::RealtimeBuffer smoothing_; void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);} - ros::Subscriber smoothing_sub_; + + realtime_tools::RealtimeBuffer delay_; + void delayCB(const std_msgs::Bool& delay) {delay_.writeFromNonRT(delay.data);} + ros::Subscriber delay_sub_; }; int main(int argc, char **argv) diff --git a/joint_trajectory_controller/test/rrbot_controllers.yaml b/joint_trajectory_controller/test/rrbot_controllers.yaml index 4623945c4..c2ac32611 100644 --- a/joint_trajectory_controller/test/rrbot_controllers.yaml +++ b/joint_trajectory_controller/test/rrbot_controllers.yaml @@ -12,3 +12,5 @@ rrbot_controller: joint2: goal: 0.01 trajectory: 0.05 + stop_trajectory_duration: 0.1 + state_publish_rate: 100