From 8b8a373fe69138f8793bb1a2c35c289ab5a13db9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 9 Feb 2024 14:11:51 +0100 Subject: [PATCH 1/4] Increasing code readability in JTC --- .../joint_trajectory_controller.hpp | 7 +- .../src/joint_trajectory_controller.cpp | 127 ++++++++++-------- 2 files changed, 75 insertions(+), 59 deletions(-) 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 b9097b1ce3..4cc8118773 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 @@ -127,7 +127,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}; @@ -171,9 +174,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 b0e93b6ecd..fca055a7b0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -125,18 +125,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 @@ -148,27 +148,32 @@ 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_); + 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_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); + current_trajectory_->set_point_before_trajectory_msg(time, state_current_); } } // 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 @@ -180,7 +185,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(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -190,8 +195,8 @@ controller_interface::return_type JointTrajectoryController::update( { 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 @@ -303,8 +308,8 @@ controller_interface::return_type JointTrajectoryController::update( 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()); } // check goal tolerance else if (!before_last_point) @@ -322,8 +327,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_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) { @@ -341,8 +346,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_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()); } } } @@ -351,16 +356,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(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) @@ -572,13 +577,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; @@ -885,9 +890,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; @@ -984,7 +988,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return CallbackReturn::SUCCESS; } @@ -1018,7 +1022,7 @@ bool JointTrajectoryController::reset() } } - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return true; } @@ -1314,6 +1318,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) { @@ -1326,33 +1331,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; } - 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) + // CHECK: if provided trajectory has points + if (trajectory.points.empty()) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.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_ERROR(get_node()->get_logger(), "Empty trajectory received."); + 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]; @@ -1367,12 +1360,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } } - if (trajectory.points.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); - return false; - } - + // 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) @@ -1388,9 +1376,32 @@ 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 trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) + { + trajectory_end_time += p.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( @@ -1404,6 +1415,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) { @@ -1446,7 +1459,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() @@ -1479,7 +1492,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 @@ -1532,7 +1545,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() From b578e59d3d13a92a0bb3fd5ec500c359477db88b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 9 Feb 2024 14:25:26 +0100 Subject: [PATCH 2/4] Fixup last_commanded_time. --- .../src/joint_trajectory_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index fca055a7b0..c9731de371 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -278,8 +278,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) @@ -910,6 +911,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()); From ca31d586c4b1032c4df8c0c0f10f7d2f6e42f61a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 12 Feb 2024 08:32:44 +0000 Subject: [PATCH 3/4] Variable renaming in the test utils --- .../test/test_trajectory_controller_utils.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 6978d0e452..485cdff884 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -149,12 +149,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_; } From b1b8161c7f5d0026d103d99c0b12d1b93dddf123 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 2 Nov 2024 19:57:41 +0000 Subject: [PATCH 4/4] Fix merge conflict --- .../src/joint_trajectory_controller.cpp | 25 ++----------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3467ec961f..a859bb25ac 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1381,24 +1381,6 @@ bool JointTrajectoryController::validate_trajectory_msg( 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) { @@ -1437,11 +1419,8 @@ bool JointTrajectoryController::validate_trajectory_msg( // in which case it can be ignored. if (trajectory_start_time.seconds() != 0.0) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } + auto const trajectory_end_time = + trajectory_start_time + trajectory.points.back().time_from_start; if (trajectory_end_time < get_node()->now()) { RCLCPP_ERROR(