From f19e7a4defdc2d7eb966e6bc38e3d941ab86a68c 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 4febbb877..72ab9aebd 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 @@ -417,6 +417,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->setAborted(rt_segment_goal->preallocated_result_); From b7b23f5e03ef7685cea1b75dd440c49cb591031e 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 72ab9aebd..3d49db87b 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 @@ -457,6 +457,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->setAborted(rt_segment_goal->preallocated_result_); rt_active_goal_.reset(); From 3d7f13439e04825b5b52b132b4963577c34a3a1d 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 | 23 ++++++++++++++++++- 1 file changed, 22 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 1a14d25a8..bacfbf18f 100644 --- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp +++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp @@ -1244,6 +1244,17 @@ TEST_F(JointTrajectoryControllerTest, pathToleranceViolation) EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout)); EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED); + // 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; @@ -1266,7 +1277,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); ros::Duration(0.5).sleep(); } @@ -1289,6 +1300,9 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation) EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout)); EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED); + // Check that we're not moving after restoring perfect control + StateConstPtr state1 = getState(); + // Restore perfect control { std_msgs::Float64 smoothing; @@ -1296,6 +1310,13 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation) smoothing_pub.publish(smoothing); ros::Duration(0.5).sleep(); } + + 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)