diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 2f0a1f4df0..1d157096e4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -125,7 +125,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr param_listener_; Params params_; + // variables for storing internal data for open-loop control trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + rclcpp::Time last_commanded_time_; + /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; @@ -170,9 +173,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr current_trajectory_ = nullptr; realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; + new_trajectory_msg_; std::shared_ptr hold_position_msg_ptr_ = nullptr; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bea37e02be..a859bb25ac 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -158,18 +158,18 @@ controller_interface::return_type JointTrajectoryController::update( // don't update goal after we sampled the trajectory to avoid any racecondition 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(); + // Check if a new trajectory message has been received from Non-RT threads + const auto current_trajectory_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 && + current_trajectory_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); } // current state update @@ -181,29 +181,33 @@ controller_interface::return_type JointTrajectoryController::update( { 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( - time, last_commanded_state_, joints_angle_wraparound_); + if (fabs(last_commanded_time_.seconds()) < std::numeric_limits::epsilon()) + { + last_commanded_time_ = time; + } + current_trajectory_->set_point_before_trajectory_msg( + last_commanded_time_, last_commanded_state_, joints_angle_wraparound_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg( + current_trajectory_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + const bool valid_point = current_trajectory_->sample( 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 @@ -215,7 +219,7 @@ controller_interface::return_type JointTrajectoryController::update( 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(); auto active_tol = active_tolerances_.readFromRT(); // have we reached the end, are not holding position, and is a timeout configured? @@ -226,8 +230,8 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(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 @@ -308,8 +312,9 @@ controller_interface::return_type JointTrajectoryController::update( assign_interface_from_point(joint_command_interface_[3], tmp_command_); } - // store the previous command. Used in open-loop control mode + // store the previous command and time used in open-loop control mode last_commanded_state_ = state_desired_; + last_commanded_time_ = time; } if (active_goal) @@ -338,8 +343,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(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()); } // check goal tolerance else if (!before_last_point) @@ -357,8 +362,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(logger, "Goal reached, success!"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -376,8 +381,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(logger, error_string.c_str()); - 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()); } } } @@ -386,16 +391,16 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(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(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) @@ -605,13 +610,13 @@ void JointTrajectoryController::query_state_service( if (has_active_trajectory()) { TrajectoryPointConstIter start_segment_itr, end_segment_itr; - response->success = traj_external_point_ptr_->sample( + response->success = current_trajectory_->sample( static_cast(request->time), interpolation_method_, state_requested, start_segment_itr, end_segment_itr); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { - if (end_segment_itr == traj_external_point_ptr_->end()) + if (end_segment_itr == current_trajectory_->end()) { RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); response->success = false; @@ -914,9 +919,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - traj_external_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; @@ -937,6 +941,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(state_current_); read_state_from_state_interfaces(last_commanded_state_); } + last_commanded_time_ = rclcpp::Time(0.0); // The controller should start by holding position at the beginning of active state add_new_trajectory_msg(set_hold_position()); @@ -1014,7 +1019,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return CallbackReturn::SUCCESS; } @@ -1048,7 +1053,7 @@ bool JointTrajectoryController::reset() } } - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return true; } @@ -1349,6 +1354,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { + // CHECK: Partial joint goals // If partial joints goals are not allowed, goal should specify all controller joints if (!params_.allow_partial_joints_goal) { @@ -1361,36 +1367,21 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if joint names are provided if (trajectory.joint_names.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); return false; } + // CHECK: if provided trajectory has points if (trajectory.points.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); return false; } - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) - { - auto const trajectory_end_time = - trajectory_start_time + trajectory.points.back().time_from_start; - if (trajectory_end_time < get_node()->now()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non-zero start time (%f) that ends in the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); - return false; - } - } - + // CHECK: If joint names are matching the joints defined for the controller for (size_t i = 0; i < trajectory.joint_names.size(); ++i) { const std::string & incoming_joint_name = trajectory.joint_names[i]; @@ -1405,6 +1396,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if trajectory ends with non-zero velocity (when option is disabled) if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) @@ -1420,9 +1412,29 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if trajectory end time is in the past (if start time defined) + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto const trajectory_end_time = + trajectory_start_time + trajectory.points.back().time_from_start; + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { + // CHECK: if time of points in the trajectory is monotonous if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( @@ -1436,6 +1448,8 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; + + // CHECK: if all required data are provided in the trajectory // This currently supports only position, velocity and acceleration inputs if (params_.allow_integration_in_goal_trajectories) { @@ -1478,7 +1492,7 @@ bool JointTrajectoryController::validate_trajectory_msg( void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + new_trajectory_msg_.writeFromNonRT(traj_msg); } void JointTrajectoryController::preempt_active_goal() @@ -1511,7 +1525,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] = current_trajectory_->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 @@ -1564,7 +1578,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( bool JointTrajectoryController::has_active_trajectory() const { - return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); + return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg(); } void JointTrajectoryController::update_pids() diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 796503c036..be14bf8dcb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -184,12 +184,12 @@ class TestableJointTrajectoryController bool has_trivial_traj() const { - return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; + return has_active_trajectory() && current_trajectory_->has_nontrivial_msg() == false; } bool has_nontrivial_traj() { - return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + return has_active_trajectory() && current_trajectory_->has_nontrivial_msg(); } double get_cmd_timeout() { return cmd_timeout_; }