From 771684540716606c5c79b4052b429796b00ec3ac Mon Sep 17 00:00:00 2001 From: Martin Oehler Date: Tue, 11 Dec 2018 10:56:34 +0100 Subject: [PATCH 1/3] Holding the current position if path tolerances were violated --- .../joint_trajectory_controller_impl.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 58be9e348..8b47113de 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -403,6 +403,10 @@ update(const ros::Time& time, const ros::Duration& period) ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]); checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true); } + + // If we violate path tolerance then hold current position + setHoldPosition(time_data.uptime, rt_active_goal_); + rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; rt_segment_goal->preallocated_result_->error_string = joint_names_[i] + " path error " + std::to_string( state_joint_error_.position[0] ); From 2e834066c705c86edc669f5f7c9055ee9a3ef570 Mon Sep 17 00:00:00 2001 From: Martin Oehler Date: Thu, 13 Dec 2018 12:35:46 +0100 Subject: [PATCH 2/3] holding position when goal tolerance was violated --- .../joint_trajectory_controller_impl.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 8b47113de..d947a2577 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -446,6 +446,9 @@ update(const ros::Time& time, const ros::Duration& period) checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance, true); } + // If we violate goal tolerances then hold current position + setHoldPosition(time_data.uptime, rt_active_goal_); + rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; rt_segment_goal->preallocated_result_->error_string = joint_names_[i] + " goal error " + std::to_string( state_joint_error_.position[0] ); rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); From 3160fe46225a09c9e21ae5d0ca6cf6602ddb7b11 Mon Sep 17 00:00:00 2001 From: Martin Oehler Date: Fri, 11 Jan 2019 16:40:46 +0100 Subject: [PATCH 3/3] extended test cases for tolerance violations to check for movement after goal abortion --- .../test/joint_trajectory_controller_test.cpp | 33 ++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp index d8ed62815..33361793e 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -1203,6 +1203,17 @@ TEST_F(JointTrajectoryControllerTest, pathToleranceViolation) // Controller continues execution, see https://github.com/ros-controls/ros_controllers/issues/48 // Make sure to restore an error-less state for the tests to continue + // Check that we're not moving + StateConstPtr state1 = getState(); + ros::Duration(0.5).sleep(); // Wait + StateConstPtr state2 = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(state1->desired.positions[i], state2->desired.positions[i], EPS); + EXPECT_NEAR(state1->desired.velocities[i], state2->desired.velocities[i], EPS); + EXPECT_NEAR(state1->desired.accelerations[i], state2->desired.accelerations[i], EPS); + } + // Restore perfect control { std_msgs::Float64 smoothing; @@ -1279,7 +1290,7 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation) // Make robot respond with a delay { std_msgs::Float64 smoothing; - smoothing.data = 0.95; + smoothing.data = 0.98; smoothing_pub.publish(smoothing); ASSERT_TRUE(waitForRobotReady()); } @@ -1307,6 +1318,9 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation) // Controller continues execution, see https://github.com/ros-controls/ros_controllers/issues/48 // Make sure to restore an error-less state for the tests to continue + // Check that we're not moving after restoring perfect control + StateConstPtr state1 = getState(); + // Restore perfect control { std_msgs::Float64 smoothing; @@ -1317,6 +1331,13 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation) ros::Duration timeout = getTrajectoryDuration(traj_goal.trajectory) + ros::Duration(TIMEOUT_TRAJ_EXECUTION_S); EXPECT_TRUE(waitForStop(timeout)); + + StateConstPtr state2 = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(state1->actual.positions[i], state2->actual.positions[i], EPS); + EXPECT_NEAR(state1->actual.velocities[i], state2->actual.velocities[i], EPS); + } } TEST_F(JointTrajectoryControllerTest, goalToleranceViolationSingleJoint) @@ -1380,11 +1401,21 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolationSingleJoint) // Controller continues execution, see https://github.com/ros-controls/ros_controllers/issues/48 // Make sure to restore an error-less state for the tests to continue + // Check that we're not moving after restoring perfect control + StateConstPtr state1 = getState(); + // Restore perfect control smoothings.data.assign({0., 0.}); smoothings_pub.publish(smoothings); ASSERT_TRUE(waitForRobotReady()); EXPECT_TRUE(waitForStop(restore_timeout)); + + StateConstPtr state2 = getState(); + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_NEAR(state1->actual.positions[i], state2->actual.positions[i], EPS); + EXPECT_NEAR(state1->actual.velocities[i], state2->actual.velocities[i], EPS); + } } int main(int argc, char** argv)