diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 4b99cee75..c23f3ac33 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -128,14 +128,14 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const const auto active_goal = *rt_active_goal_.readFromRT(); // Check if a new external message has been received from nonRT threads - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + auto current_external_msg = current_trajectory_->get_trajectory_msg(); + auto new_external_msg = new_trajectory_msg_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) if (current_external_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); + current_trajectory_->update(*new_external_msg); } // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not @@ -166,22 +166,22 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const bool first_sample = false; // if sampling the first time, set the point before you sample - if (!traj_external_point_ptr_->is_sampled_already()) { + if (!current_trajectory_->is_sampled_already()) { first_sample = true; if (params_.open_loop_control) { - traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_); + current_trajectory_->set_point_before_trajectory_msg(traj_time, last_commanded_state_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_); + current_trajectory_->set_point_before_trajectory_msg(traj_time, state_current_); } } // find segment for current timestamp joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample(traj_time, interpolation_method_, state_desired_, - start_segment_itr, end_segment_itr); + const bool valid_point = current_trajectory_->sample(traj_time, interpolation_method_, state_desired_, + start_segment_itr, end_segment_itr); if (valid_point) { - const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + const rclcpp::Time traj_start = current_trajectory_->time_from_start(); // this is the time instance // - started with the first segment: when the first point will be reached (in the future) // - later: when the point of the current segment was reached @@ -193,7 +193,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + const bool before_last_point = end_segment_itr != current_trajectory_->end(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -201,8 +201,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const time_difference > cmd_timeout_) { RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // Check state/goal tolerance @@ -292,8 +292,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } else if (!before_last_point) { // check goal tolerance if (!outside_goal_tolerance) { auto res = std::make_shared(); @@ -306,8 +306,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } else if (!within_goal_time) { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); @@ -320,21 +320,21 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", time_difference); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } } } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied (will stay in this state until new message arrives)