@@ -178,8 +178,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
178178 if (!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) {
179179 RCLCPP_WARN (logger, " Aborted due to command timeout" );
180180
181- traj_msg_external_point_ptr_ .reset ();
182- traj_msg_external_point_ptr_ .initRT (set_hold_position ());
181+ new_trajectory_msg_ .reset ();
182+ new_trajectory_msg_ .initRT (set_hold_position ());
183183 }
184184
185185 // Check state/goal tolerance
@@ -313,8 +313,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
313313
314314 RCLCPP_WARN (logger, " %s" , error_string.c_str ());
315315
316- traj_msg_external_point_ptr_ .reset ();
317- traj_msg_external_point_ptr_ .initRT (set_hold_position ());
316+ new_trajectory_msg_ .reset ();
317+ new_trajectory_msg_ .initRT (set_hold_position ());
318318 }
319319 }
320320 } else if (tolerance_violated_while_moving && !rt_has_pending_goal_) {
@@ -326,8 +326,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
326326 } else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_) {
327327 RCLCPP_ERROR (logger, " Exceeded goal_time_tolerance: holding position..." );
328328
329- traj_msg_external_point_ptr_ .reset ();
330- traj_msg_external_point_ptr_ .initRT (set_hold_position ());
329+ new_trajectory_msg_ .reset ();
330+ new_trajectory_msg_ .initRT (set_hold_position ());
331331 }
332332 // else, run another cycle while waiting for outside_goal_tolerance
333333 // to be satisfied (will stay in this state until new message arrives)
0 commit comments