From f46b989cefb1c7ea46fb6f744081c9007213aab7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 19 Nov 2023 21:49:35 +0000 Subject: [PATCH] Set time of success position --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d6048a5bac..985364de13 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1475,6 +1475,7 @@ JointTrajectoryController::set_success_trajectory_point() // set last command to be repeated at success, no matter if it has nonzero velocity or // acceleration hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); // set flag, otherwise tolerances will be checked with success_trajectory_point too rt_is_holding_.writeFromNonRT(true);