From a6b8a1c1606b466c9b1aee505c4abc1bb9096e1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 24 Jul 2023 19:21:43 +0200 Subject: [PATCH 01/22] [Doc] Fix links (backport #715) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 6 +++--- doc/writing_new_controller.rst | 2 +- joint_trajectory_controller/README.md | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 6bbf2e957a..6a829c46f9 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! -- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/humble/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://control.ros.org/humble/doc/getting_started/getting_started.html#building-from-source). - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork) @@ -53,8 +53,8 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! Furthermore, you find helpful resources here: -* [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) -* [ROS2 Tutorials](https://docs.ros.org/en/foxy/Tutorials.html) +* [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) +* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) **Good luck with your first issue!** diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 5d8797628e..501c231def 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -5,7 +5,7 @@ Writing a new controller ======================== -In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. +In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller. 1. **Preparing package** diff --git a/joint_trajectory_controller/README.md b/joint_trajectory_controller/README.md index 9bffcc3566..874176d228 100644 --- a/joint_trajectory_controller/README.md +++ b/joint_trajectory_controller/README.md @@ -2,4 +2,4 @@ The package implements controllers to interpolate joint's trajectory. -For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/). +For detailed documentation check the `docs` folder or [ros2_control documentation](https://control.ros.org/). From 4abddd59ab8758d7d319b226c1dac44d3f6756eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 15 Aug 2023 13:37:52 +0200 Subject: [PATCH 02/22] [JTC] Explicitly set hold position (backport #558) --- .../doc/parameters.rst | 6 + joint_trajectory_controller/doc/userdoc.rst | 3 +- .../joint_trajectory_controller.hpp | 7 +- .../trajectory.hpp | 3 + .../src/joint_trajectory_controller.cpp | 146 +++++++++++------ ...oint_trajectory_controller_parameters.yaml | 5 + .../src/trajectory.cpp | 5 + .../test/test_trajectory_actions.cpp | 149 ++++++++---------- .../test/test_trajectory_controller.cpp | 88 +++++++---- .../test/test_trajectory_controller_utils.hpp | 85 +++++++++- 10 files changed, 331 insertions(+), 166 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index f62320870c..eaf21d6519 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -62,6 +62,12 @@ open_loop_control (boolean) Default: false +start_with_holding (bool) + If true, start with holding position after activation. Otherwise, no command will be sent until + the first trajectory is received. + + Default: true + allow_nonzero_velocity_at_trajectory_end (boolean) If false, the last velocity point has to be zero or the goal will be rejected. diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 92f0f9629d..bec732037b 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -105,7 +105,6 @@ When an active action goal is preempted by another command coming from the actio Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. - .. _ROS 2 interface: Description of controller's interfaces @@ -162,7 +161,7 @@ Subscriber [#f1]_ Topic for commanding the controller The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. -The controller's path and goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. +The goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. If state tolerances are violated, the trajectory is aborted and the current position is held. Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. 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 e9738d7f48..b13c12cef8 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 @@ -180,7 +180,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; std::shared_ptr traj_msg_home_ptr_ = nullptr; @@ -205,6 +204,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp_action::Server::SharedPtr action_server_; RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + realtime_tools::RealtimeBuffer rt_has_pending_goal_; ///< Is there a pending action goal? rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); @@ -247,11 +247,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_hold_position(); + std::shared_ptr set_hold_position(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_active_trajectory(); + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 230d0198f7..d2d0dc9dbd 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -130,6 +130,9 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_trajectory_msg() const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_nontrivial_msg() const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr get_trajectory_msg() const { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cff2918b75..08ab7eea77 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -157,17 +157,21 @@ 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(); - if (current_external_msg != *new_external_msg) + // 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); - // set the active trajectory pointer to the new goal - traj_point_active_ptr_ = &traj_external_point_ptr_; } // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not @@ -185,29 +189,28 @@ controller_interface::return_type JointTrajectoryController::update( state_current_.time_from_start.set__sec(0); read_state_from_hardware(state_current_); - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + // currently carrying out a trajectory. + if (has_active_trajectory()) { bool first_sample = false; // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) + if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; if (params_.open_loop_control) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); } else { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); + traj_external_point_ptr_->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_point_active_ptr_) - ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + const bool valid_point = traj_external_point_ptr_->sample( + time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); if (valid_point) { @@ -215,7 +218,7 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; double time_difference = 0.0; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); + const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) @@ -242,7 +245,7 @@ controller_interface::return_type JointTrajectoryController::update( if (default_tolerances_.goal_time_tolerance != 0.0) { // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); + const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time(); const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; time_difference = time.seconds() - traj_end.seconds(); @@ -299,7 +302,6 @@ controller_interface::return_type JointTrajectoryController::update( last_commanded_state_ = state_desired_; } - const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { // send feedback @@ -315,20 +317,20 @@ controller_interface::return_type JointTrajectoryController::update( // check abort if (tolerance_violated_while_moving) { - set_hold_position(); auto result = std::make_shared(); - - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; + rt_has_pending_goal_.writeFromNonRT(false); - // check goal tolerance + 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()); } + // check goal tolerance else if (!before_last_point) { if (!outside_goal_tolerance) @@ -339,32 +341,42 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // remove the active trajectory pointer so that we stop commanding the hardware - traj_point_active_ptr_ = nullptr; + rt_has_pending_goal_.writeFromNonRT(false); RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + + traj_msg_external_point_ptr_.reset(); + traj_msg_external_point_ptr_.initRT(set_hold_position()); } else if (!within_goal_time) { - set_hold_position(); auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_has_pending_goal_.writeFromNonRT(false); + 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()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied or violated within the goal_time_tolerance } } - else if (tolerance_violated_while_moving) + else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { - set_hold_position(); + // 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()); } } } @@ -570,17 +582,16 @@ void JointTrajectoryController::query_state_service( const auto active_goal = *rt_active_goal_.readFromRT(); response->name = params_.joints; trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; - if ((traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())) + if (has_active_trajectory()) { TrajectoryPointConstIter start_segment_itr, end_segment_itr; - response->success = (*traj_point_active_ptr_) - ->sample( - static_cast(request->time), interpolation_method_, - state_requested, start_segment_itr, end_segment_itr); + response->success = traj_external_point_ptr_->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_point_active_ptr_)->end()) + if (end_segment_itr == traj_external_point_ptr_->end()) { RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); response->success = false; @@ -977,7 +988,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::shared_ptr()); subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; last_state_publish_time_ = get_node()->now(); // Initialize current state storage if hardware state has tracking offset @@ -995,13 +1005,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_commanded_state_ = state; } + // Should the controller start by holding position after on_configure? + if (params_.start_with_holding) + { + add_new_trajectory_msg(set_hold_position()); + } + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { - // TODO(anyone): How to halt when using effort commands? for (size_t index = 0; index < dof_; ++index) { if (has_position_command_interface_) @@ -1015,6 +1030,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( joint_command_interface_[1][index].get().set_value(0.0); } + if (has_acceleration_command_interface_) + { + joint_command_interface_[2][index].get().set_value(0.0); + } + + // TODO(anyone): How to halt when using effort commands? if (has_effort_command_interface_) { joint_command_interface_[3][index].get().set_value(0.0); @@ -1030,6 +1051,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; + traj_external_point_ptr_.reset(); + traj_home_point_ptr_.reset(); + traj_msg_home_ptr_.reset(); + return CallbackReturn::SUCCESS; } @@ -1040,7 +1065,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( if (traj_home_point_ptr_ != nullptr) { traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; + traj_external_point_ptr_ = traj_home_point_ptr_; } return CallbackReturn::SUCCESS; @@ -1071,7 +1096,6 @@ bool JointTrajectoryController::reset() // iterator has no default value // prev_traj_point_ptr_; - traj_point_active_ptr_ = nullptr; traj_external_point_ptr_.reset(); traj_home_point_ptr_.reset(); traj_msg_home_ptr_.reset(); @@ -1205,17 +1229,17 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal && active_goal->gh_ == goal_handle) { - // Controller uptime - // Enter hold current position mode - set_hold_position(); - - RCLCPP_DEBUG( + RCLCPP_INFO( get_node()->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled + rt_has_pending_goal_.writeFromNonRT(false); auto action_res = std::make_shared(); active_goal->setCanceled(action_res); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + + // Enter hold current position mode + add_new_trajectory_msg(set_hold_position()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -1223,6 +1247,9 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback void JointTrajectoryController::goal_accepted_callback( std::shared_ptr> goal_handle) { + // mark a pending goal + rt_has_pending_goal_.writeFromNonRT(true); + // Update new trajectory { preempt_active_goal(); @@ -1511,7 +1538,7 @@ void JointTrajectoryController::preempt_active_goal() const auto active_goal = *rt_active_goal_.readFromNonRT(); if (active_goal) { - set_hold_position(); + add_new_trajectory_msg(set_hold_position()); auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled due to new incoming action."); @@ -1520,13 +1547,35 @@ void JointTrajectoryController::preempt_active_goal() } } -void JointTrajectoryController::set_hold_position() +std::shared_ptr +JointTrajectoryController::set_hold_position() { - trajectory_msgs::msg::JointTrajectory empty_msg; - empty_msg.header.stamp = rclcpp::Time(0); + // Command to stay at current position + trajectory_msgs::msg::JointTrajectory current_pose_msg; + current_pose_msg.header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + current_pose_msg.joint_names = params_.joints; + current_pose_msg.points.push_back(state_current_); + current_pose_msg.points[0].velocities.clear(); + current_pose_msg.points[0].accelerations.clear(); + current_pose_msg.points[0].effort.clear(); + if (has_velocity_command_interface_) + { + // ensure no velocity (PID will fix this) + current_pose_msg.points[0].velocities.resize(dof_, 0.0); + } + if (has_acceleration_command_interface_) + { + // ensure no acceleration + current_pose_msg.points[0].accelerations.resize(dof_, 0.0); + } + if (has_effort_command_interface_) + { + // ensure no explicit effort (PID will fix this) + current_pose_msg.points[0].effort.resize(dof_, 0.0); + } - auto traj_msg = std::make_shared(empty_msg); - add_new_trajectory_msg(traj_msg); + return std::make_shared(current_pose_msg); } bool JointTrajectoryController::contains_interface_type( @@ -1571,6 +1620,11 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } +bool JointTrajectoryController::has_active_trajectory() +{ + return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index bb0b45bb80..7273e90d05 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -74,6 +74,11 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } + start_with_holding: { + type: bool, + default_value: true, + description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", + } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: true, diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 06b3ca6e9b..fae4c41ff9 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -351,4 +351,9 @@ rclcpp::Time Trajectory::time_from_start() const { return trajectory_start_time_ bool Trajectory::has_trajectory_msg() const { return trajectory_msg_.get() != nullptr; } +bool Trajectory::has_nontrivial_msg() const +{ + return has_trajectory_msg() && trajectory_msg_->points.size() > 1; +} + } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fdea551c30..2c28319d52 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -224,15 +224,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::shared_future gh_future; // send goal + std::vector point_positions{1.0, 2.0, 3.0}; { std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; + point.positions = point_positions; points.push_back(point); @@ -243,12 +242,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_EQ(1.0, joint_pos_[0]); - EXPECT_EQ(2.0, joint_pos_[1]); - EXPECT_EQ(3.0, joint_pos_[2]); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectHoldingPoint(point_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) @@ -265,24 +265,21 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::shared_future gh_future; // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -293,12 +290,12 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_TRUE(gh_future.get()); EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(points_positions.at(1)); } /** @@ -318,15 +315,14 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) std::shared_future gh_future; // send goal + std::vector> points_positions{{{1.0, 2.0, 3.0}}}; { std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); point.positions.resize(joint_names_.size()); - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; + point.positions = points_positions.at(0); points.push_back(point); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -338,12 +334,12 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(points_positions.at(0)); } /** @@ -370,24 +366,21 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) std::shared_future gh_future; // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -400,12 +393,12 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD); - } + // run an update + updateController(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -417,29 +410,28 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint2.trajectory", state_tol), rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; - SetUpExecutor(params); + // separate command from states -> immediate state tolerance fail + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); std::shared_future gh_future; // send goal + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.0); point1.positions.resize(joint_names_.size()); - point1.positions[0] = 4.0; - point1.positions[1] = 5.0; - point1.positions[2] = 6.0; + point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.1); point2.positions.resize(joint_names_.size()); - point2.positions[0] = 7.0; - point2.positions[1] = 8.0; - point2.positions[2] = 9.0; + point2.positions = points_positions.at(1); points.push_back(point2); gh_future = sendActionGoal(points, 1.0, goal_options_); @@ -452,15 +444,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD); - } + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -503,15 +493,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding the last received goal + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(4.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(5.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(6.0, joint_pos_[2], COMMON_THRESHOLD); - } + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -523,13 +511,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol rclcpp::Parameter("constraints.joint2.trajectory", state_tol), rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; - SetUpExecutor(params); + // separate command from states -> goal won't never be reached + bool separate_cmd_and_state_values = true; + SetUpExecutor(params, separate_cmd_and_state_values); SetUpControllerHardware(); - const double init_pos1 = joint_pos_[0]; - const double init_pos2 = joint_pos_[1]; - const double init_pos3 = joint_pos_[2]; - std::shared_future gh_future; // send goal { @@ -553,15 +539,13 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED, common_action_result_code_); - // run an update, it should be holding + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD); - } + // it should be holding the position (being the initial one) + // i.e., active but trivial trajectory (one point only) + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -601,19 +585,14 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - const double prev_pos1 = joint_pos_[0]; - const double prev_pos2 = joint_pos_[1]; - const double prev_pos3 = joint_pos_[2]; + std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; - // run an update, it should be holding + // run an update updateController(rclcpp::Duration::from_seconds(0.01)); - if (traj_controller_->has_position_command_interface()) - { - EXPECT_EQ(prev_pos1, joint_pos_[0]); - EXPECT_EQ(prev_pos2, joint_pos_[1]); - EXPECT_EQ(prev_pos3, joint_pos_[2]); - } + // it should be holding the last position, + // i.e., active but trivial trajectory (one point only) + expectHoldingPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 72cd2e8ca9..a08648db5e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -285,44 +285,38 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // first update traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - // wait so controller process the second point when deactivated + // wait for reaching the first point + // controller would process the second point when deactivated below traj_controller_->update( rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); - // deactivated - state = traj_controller_->get_node()->deactivate(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - - const auto allowed_delta = 0.05; + EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); + EXPECT_NEAR(points.at(0).at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(2), joint_pos_[2], COMMON_THRESHOLD); } - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_LE(0.0, joint_eff_[0]); - EXPECT_LE(0.0, joint_eff_[1]); - EXPECT_LE(0.0, joint_eff_[2]); - } + // deactivate + std::vector deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; + state = traj_controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + // it should be holding the current point + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectHoldingPointDeactivated(deactivated_positions); - // reactivated - // wait so controller process the third point when reactivated + // reactivate + // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - ActivateTrajectoryController(); + + ActivateTrajectoryController(false, deactivated_positions); state = traj_controller_->get_state(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); - // TODO(christophfroehlich) add test if there is no active trajectory after - // reactivation once #558 or #609 got merged (needs methods for TestableJointTrajectoryController) + // it should still be holding the position at time of deactivation + // i.e., active but trivial trajectory (one point only) + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); + expectHoldingPoint(deactivated_positions); executor.cancel(); } @@ -476,6 +470,45 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief check if hold on startup is deactivated + */ +TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); + SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // after startup without start_with_holding being set, we expect no active trajectory + ASSERT_FALSE(traj_controller_->has_active_traj()); + + executor.cancel(); +} + +/** + * @brief check if hold on startup + */ +TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); + SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // after startup with start_with_holding being set, we expect an active trajectory: + ASSERT_TRUE(traj_controller_->has_active_traj()); + // one point, being the position at startup + std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; + expectHoldingPoint(initial_positions); + + executor.cancel(); +} + // Floating-point value comparison threshold const double EPS = 1e-6; /** @@ -1255,7 +1288,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; - std::cout << "Sending old trajectory" << std::endl; publish(time_from_start, points_new, new_traj_start); waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 874a814a5d..353d8e8d1c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/trajectory.hpp" namespace { @@ -123,6 +124,13 @@ class TestableJointTrajectoryController bool is_open_loop() { return params_.open_loop_control; } + bool has_active_traj() { return has_active_trajectory(); } + + bool has_trivial_traj() + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -224,7 +232,9 @@ class TrajectoryControllerTest : public ::testing::Test ActivateTrajectoryController(separate_cmd_and_state_values); } - void ActivateTrajectoryController(bool separate_cmd_and_state_values = false) + void ActivateTrajectoryController( + bool separate_cmd_and_state_values = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -258,14 +268,14 @@ class TrajectoryControllerTest : public ::testing::Test // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_POS_JOINTS[i]); + cmd_interfaces.back().set_value(initial_pos_joints[i]); cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]); cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]); - joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; + joint_state_pos_[i] = initial_pos_joints[i]; joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; state_interfaces.emplace_back(pos_state_interfaces_.back()); @@ -456,6 +466,75 @@ class TrajectoryControllerTest : public ::testing::Test } void test_state_publish_rate_target(int target_msg_count); + void expectHoldingPoint(std::vector point) + { + // it should be holding the given point + // i.e., active but trivial trajectory (one point only) + EXPECT_TRUE(traj_controller_->has_trivial_traj()); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + + void expectHoldingPointDeactivated(std::vector point) + { + // it should be holding the given point, but no active trajectory + EXPECT_FALSE(traj_controller_->has_active_traj()); + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + std::string controller_name_; std::vector joint_names_; From 63ea0182f29cd5262338bbbe8617d84031c5e386 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 16 Aug 2023 17:01:34 +0200 Subject: [PATCH 03/22] [JTC] Tolerance tests + Hold on time violation (backport #613) * Add new test to ensure that controller goes into position holding when tolerances are violated * Hold position if goal_time is exceeded with topic interface * Fix hold on time-violation --- .../src/joint_trajectory_controller.cpp | 14 ++++- .../test/test_trajectory_controller.cpp | 61 +++++++++++++++++++ 2 files changed, 72 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 08ab7eea77..44ee3f8729 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -365,19 +365,27 @@ controller_interface::return_type JointTrajectoryController::update( traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); } - // else, run another cycle while waiting for outside_goal_tolerance - // to be satisfied or violated within the goal_time_tolerance } } 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()); } + 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()); + } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied (will stay in this state until new message arrives) + // or outside_goal_tolerance violated within the goal_time_tolerance } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a08648db5e..fa4761a814 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1666,6 +1666,67 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) +{ + // set joint tolerance parameters + const double state_tol = 0.0001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; + + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, false, {params}, true); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectHoldingPoint(joint_state_pos_); +} + +TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) +{ + // set joint tolerance parameters + const double goal_tol = 0.1; + // set very small goal_time so that goal_time is violated + const double goal_time = 0.000001; + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", goal_tol), + rclcpp::Parameter("constraints.joint2.goal", goal_tol), + rclcpp::Parameter("constraints.joint3.goal", goal_tol), + rclcpp::Parameter("constraints.goal_time", goal_time)}; + + rclcpp::executors::MultiThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, false, {params}, true); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectHoldingPoint(joint_state_pos_); +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, From 6c3b73eb39d017055eb202c9e4587b88b3ef0955 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Sep 2023 11:28:37 +0200 Subject: [PATCH 04/22] [JTC] Make most parameters read-only (backport #771) --- joint_trajectory_controller/CMakeLists.txt | 3 +- .../src/joint_trajectory_controller.cpp | 2 +- ...oint_trajectory_controller_parameters.yaml | 7 +++ .../test_joint_trajectory_controller.yaml | 12 ----- .../test/test_trajectory_actions.cpp | 3 +- .../test/test_trajectory_controller.cpp | 53 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 35 +++++------- 7 files changed, 49 insertions(+), 66 deletions(-) delete mode 100644 joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 73d01ef132..60116f2b0a 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -58,8 +58,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory joint_trajectory_controller) ament_add_gmock(test_trajectory_controller - test/test_trajectory_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) + test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_link_libraries(test_trajectory_controller joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 44ee3f8729..13a99664f9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1013,7 +1013,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_commanded_state_ = state; } - // Should the controller start by holding position after on_configure? + // Should the controller start by holding position at the beginning of active state? if (params_.start_with_holding) { add_new_trajectory_msg(set_hold_position()); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 7273e90d05..cbe388c1fd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -3,6 +3,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of joints used by the controller", + read_only: true, validation: { unique<>: null, } @@ -11,6 +12,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of the commanding joints used by the controller", + read_only: true, validation: { unique<>: null, } @@ -19,6 +21,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of command interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], @@ -29,6 +32,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of state interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], @@ -44,6 +48,7 @@ joint_trajectory_controller: type: bool, default_value: false, description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + read_only: true, } allow_integration_in_goal_trajectories: { type: bool, @@ -62,6 +67,7 @@ joint_trajectory_controller: type: double, default_value: 20.0, description: "Rate status changes will be monitored", + read_only: true, validation: { gt_eq: [0.1] } @@ -70,6 +76,7 @@ joint_trajectory_controller: type: string, default_value: "splines", description: "The type of interpolation to use, if any", + read_only: true, validation: { one_of<>: [["splines", "none"]], } diff --git a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml b/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml deleted file mode 100644 index 09a134e06a..0000000000 --- a/joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml +++ /dev/null @@ -1,12 +0,0 @@ -test_joint_trajectory_controller: - joints: - - joint1 - - joint2 - - joint3 - - command_interfaces: - - position - - state_interfaces: - - position - - velocity diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 2c28319d52..fc24c2c029 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -72,8 +72,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest { setup_executor_ = true; - SetUpAndActivateTrajectoryController( - executor_, true, parameters, separate_cmd_and_state_values); + SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values); SetUpActionClient(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index fa4761a814..4d8962e272 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -138,11 +138,9 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor); - // set command_joints parameter const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); - traj_controller_->get_node()->set_parameter({command_joint_names_param}); + SetUpTrajectoryController(executor, {command_joint_names_param}); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -206,7 +204,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) rclcpp::executors::MultiThreadedExecutor executor; std::vector params = { rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params); + SetUpAndActivateTrajectoryController(executor, params); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -252,12 +250,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor, false); + SetUpTrajectoryController(executor); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - SetParameters(); traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -324,7 +321,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToStateLegacy(); updateController(); @@ -376,7 +373,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency) TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); updateController(); @@ -478,7 +475,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateController(rclcpp::Duration(FIRST_POINT_TIME)); @@ -496,7 +493,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) rclcpp::executors::MultiThreadedExecutor executor; rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateController(rclcpp::Duration(FIRST_POINT_TIME)); @@ -520,7 +517,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) constexpr double k_p = 10.0; std::vector params = { rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false); + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -629,7 +626,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) constexpr double k_p = 10.0; std::vector params = { rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true); + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -750,7 +747,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou rclcpp::Parameter state_publish_rate_param( "state_publish_rate", static_cast(target_msg_count)); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param}); + SetUpAndActivateTrajectoryController(executor, {state_publish_rate_param}); auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); @@ -819,7 +816,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}, true); + SetUpAndActivateTrajectoryController(executor, {}, true); subscribeToState(); size_t n_joints = joint_names_.size(); @@ -945,7 +942,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; @@ -1017,7 +1014,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; @@ -1099,7 +1096,7 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController( - executor, true, {partial_joints_parameters, allow_integration_parameters}); + executor, {partial_joints_parameters, allow_integration_parameters}); trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; @@ -1160,7 +1157,7 @@ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted { rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {allow_integration_parameters}); + SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters}); trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; @@ -1222,7 +1219,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) { rclcpp::executors::SingleThreadedExecutor executor; rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); subscribeToState(); @@ -1265,7 +1262,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations @@ -1295,7 +1292,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1328,7 +1325,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur { rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); subscribeToState(); RCLCPP_WARN( @@ -1388,7 +1385,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1489,7 +1486,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e { rclcpp::executors::SingleThreadedExecutor executor; rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1587,7 +1584,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co joint_vel_[i] = std::numeric_limits::quiet_NaN(); joint_acc_[i] = std::numeric_limits::quiet_NaN(); } - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1634,7 +1631,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co joint_vel_[i] = 0.25 + i; joint_acc_[i] = 0.02 + i / 10.0; } - SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1676,7 +1673,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, false, {params}, true); + SetUpAndActivateTrajectoryController(executor, params, true); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1708,7 +1705,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.goal_time", goal_time)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, false, {params}, true); + SetUpAndActivateTrajectoryController(executor, params, true); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 353d8e8d1c..33693cc4c6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -162,32 +162,26 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); } - void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true) + void SetUpTrajectoryController( + rclcpp::Executor & executor, const std::vector & parameters = {}) { traj_controller_ = std::make_shared(); - if (use_local_parameters) - { - traj_controller_->set_joint_names(joint_names_); - traj_controller_->set_command_interfaces(command_interface_types_); - traj_controller_->set_state_interfaces(state_interface_types_); - } - auto ret = traj_controller_->init(controller_name_); + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_)); + parameter_overrides.push_back( + rclcpp::Parameter("command_interfaces", command_interface_types_)); + parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + auto ret = traj_controller_->init(controller_name_, "", node_options); if (ret != controller_interface::return_type::OK) { FAIL(); } executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - SetParameters(); - } - - void SetParameters() - { - auto node = traj_controller_->get_node(); - const rclcpp::Parameter joint_names_param("joints", joint_names_); - const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_); - const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); - node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } void SetPidParameters( @@ -210,12 +204,11 @@ class TrajectoryControllerTest : public ::testing::Test } void SetUpAndActivateTrajectoryController( - rclcpp::Executor & executor, bool use_local_parameters = true, - const std::vector & parameters = {}, + rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, bool normalize_error = false) { - SetUpTrajectoryController(executor, use_local_parameters); + SetUpTrajectoryController(executor); // set pid parameters before configure SetPidParameters(k_p, ff, normalize_error); From 14438cd63285792371731d2546bfc584a4847bc5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Sep 2023 17:40:15 +0200 Subject: [PATCH 05/22] [JTC] Add note on goal_time=0 in docs (backport #773) --- joint_trajectory_controller/doc/parameters.rst | 1 + .../src/joint_trajectory_controller_parameters.yaml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index eaf21d6519..bb896fc9b7 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -83,6 +83,7 @@ constraints.stopped_velocity_tolerance (double) constraints.goal_time (double) Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. + If set to zero, the controller will wait a potentially infinite amount of time. Default: 0.0 (not checked) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index cbe388c1fd..00d4517308 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -138,7 +138,8 @@ joint_trajectory_controller: goal_time: { type: double, default_value: 0.0, - description: "Time tolerance for achieving trajectory goal before or after commanded time.", + description: "Time tolerance for achieving trajectory goal before or after commanded time. + If set to zero, the controller will wait a potentially infinite amount of time.", validation: { gt_eq: [0.0], } From 84684e5515dead5a148abefe440e7a1dbc2aea8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Sep 2023 11:54:55 +0200 Subject: [PATCH 06/22] [JTC] Fix hold position mode with goal_time>0 (backport #758) --- .../joint_trajectory_controller.hpp | 4 + .../src/joint_trajectory_controller.cpp | 64 +++++++++------ .../test/test_trajectory_actions.cpp | 24 +++++- .../test/test_trajectory_controller.cpp | 12 ++- .../test/test_trajectory_controller_utils.hpp | 81 ++++++++++++++----- 5 files changed, 132 insertions(+), 53 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 b13c12cef8..5a2236f437 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 @@ -173,6 +173,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; + realtime_tools::RealtimeBuffer rt_is_holding_; ///< are we holding position? // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = @@ -186,6 +187,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; + std::shared_ptr hold_position_msg_ptr_ = nullptr; + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; @@ -274,6 +277,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); + void init_hold_position_msg(); void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 13a99664f9..e935d27a66 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -230,7 +230,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) + state_error_, index, default_tolerances_.state_tolerance[index], false) && + *(rt_is_holding_.readFromRT()) == false) { tolerance_violated_while_moving = true; } @@ -238,7 +239,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && + *(rt_is_holding_.readFromRT()) == false) { outside_goal_tolerance = true; @@ -831,6 +833,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( logger, "Using '%s' interpolation method.", interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + // prepare hold_position_msg + init_hold_position_msg(); + + // create subscriber and publishers joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), @@ -1018,6 +1024,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { add_new_trajectory_msg(set_hold_position()); } + rt_is_holding_.writeFromNonRT(true); return CallbackReturn::SUCCESS; } @@ -1203,6 +1210,7 @@ void JointTrajectoryController::topic_callback( if (subscriber_is_active_) { add_new_trajectory_msg(msg); + rt_is_holding_.writeFromNonRT(false); } }; @@ -1265,6 +1273,7 @@ void JointTrajectoryController::goal_accepted_callback( std::make_shared(goal_handle->get_goal()->trajectory); add_new_trajectory_msg(traj_msg); + rt_is_holding_.writeFromNonRT(false); } // Update the active goal @@ -1559,31 +1568,12 @@ std::shared_ptr JointTrajectoryController::set_hold_position() { // Command to stay at current position - trajectory_msgs::msg::JointTrajectory current_pose_msg; - current_pose_msg.header.stamp = - rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately - current_pose_msg.joint_names = params_.joints; - current_pose_msg.points.push_back(state_current_); - current_pose_msg.points[0].velocities.clear(); - current_pose_msg.points[0].accelerations.clear(); - current_pose_msg.points[0].effort.clear(); - if (has_velocity_command_interface_) - { - // ensure no velocity (PID will fix this) - current_pose_msg.points[0].velocities.resize(dof_, 0.0); - } - if (has_acceleration_command_interface_) - { - // ensure no acceleration - current_pose_msg.points[0].accelerations.resize(dof_, 0.0); - } - if (has_effort_command_interface_) - { - // ensure no explicit effort (PID will fix this) - current_pose_msg.points[0].effort.resize(dof_, 0.0); - } + hold_position_msg_ptr_->points[0].positions = state_current_.positions; - return std::make_shared(current_pose_msg); + // set flag, otherwise tolerances will be checked with holding position too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; } bool JointTrajectoryController::contains_interface_type( @@ -1633,6 +1623,28 @@ bool JointTrajectoryController::has_active_trajectory() return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::init_hold_position_msg() +{ + hold_position_msg_ptr_ = std::make_shared(); + hold_position_msg_ptr_->header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + hold_position_msg_ptr_->joint_names = params_.joints; + hold_position_msg_ptr_->points.resize(1); // a trivial msg only + hold_position_msg_ptr_->points[0].velocities.clear(); + hold_position_msg_ptr_->points[0].accelerations.clear(); + hold_position_msg_ptr_->points[0].effort.clear(); + if (has_velocity_command_interface_ || has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns velocity points in any case + hold_position_msg_ptr_->points[0].velocities.resize(dof_, 0.0); + } + if (has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns acceleration points in any case + hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fc24c2c029..371be914e2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, double kp = 0.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values); + SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); SetUpActionClient(); @@ -247,7 +247,15 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) // note: the action goal also is a trivial trajectory - expectHoldingPoint(point_positions); + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(point_positions); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) @@ -294,7 +302,15 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(points_positions.at(1)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } } /** diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 4d8962e272..69d14efa5f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1673,7 +1673,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, params, true); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, params, true, kp); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1705,7 +1706,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.goal_time", goal_time)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, params, true); + SetUpAndActivateTrajectoryController(executor, params, true, 1.0); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1722,6 +1723,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); + + // what happens if we wait longer but it harms the tolerance again? + auto hold_position = joint_state_pos_; + joint_state_pos_.at(0) = -3.3; + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // it should be still holding the old point + expectHoldingPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 33693cc4c6..d222e90e9c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -38,6 +38,12 @@ const std::vector INITIAL_POS_JOINTS = { const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; + +bool is_same_sign_or_zero(double val1, double val2) +{ + return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); +} + } // namespace namespace test_trajectory_controllers @@ -465,32 +471,65 @@ class TrajectoryControllerTest : public ::testing::Test // i.e., active but trivial trajectory (one point only) EXPECT_TRUE(traj_controller_->has_trivial_traj()); - if (traj_controller_->has_position_command_interface()) + if (traj_controller_->use_closed_loop_pid_adapter() == false) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); - } + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + } - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); - } + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } - if (traj_controller_->has_acceleration_command_interface()) - { - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } - if (traj_controller_->has_effort_command_interface()) + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } + } + else { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); + // velocity or effort PID? + // velocity setpoint is always zero -> feedforward term does not have an effect + // --> set kp > 0.0 in test + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0])) + << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is " + << joint_vel_[0]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1])) + << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is " + << joint_vel_[1]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2])) + << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is " + << joint_vel_[2]; + } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0])) + << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is " + << joint_eff_[0]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1])) + << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is " + << joint_eff_[1]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2])) + << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is " + << joint_eff_[2]; + } } } From 4b9e25b4fbb0b73f0e0c7c0649a2a4905c467da5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 14 Sep 2023 18:20:07 +0200 Subject: [PATCH 07/22] [JTC] Add time-out for trajectory interfaces (backport #609) --- .../doc/parameters.rst | 10 ++ .../joint_trajectory_controller.hpp | 5 +- .../trajectory.hpp | 3 - .../src/joint_trajectory_controller.cpp | 50 ++++++- ...oint_trajectory_controller_parameters.yaml | 8 + .../test/test_trajectory_controller.cpp | 141 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 7 + 7 files changed, 212 insertions(+), 12 deletions(-) diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index bb896fc9b7..0866aab8b8 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -73,6 +73,16 @@ allow_nonzero_velocity_at_trajectory_end (boolean) Default: true +cmd_timeout (double) + Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + ``cmd_timeout`` must be greater than ``constraints.goal_time``, + otherwise ignored. + + If zero, timeout is deactivated" + + Default: 0.0 + constraints (structure) Default values for tolerances if no explicit values are states in JointTrajectory message. 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 5a2236f437..522164776e 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 @@ -173,7 +173,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; - realtime_tools::RealtimeBuffer rt_is_holding_; ///< are we holding position? + // Timeout to consider commands old + double cmd_timeout_; + // Are we holding position? + realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d2d0dc9dbd..3bd4873a31 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -139,9 +139,6 @@ class Trajectory return trajectory_msg_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp::Time get_trajectory_start_time() const { return trajectory_start_time_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e935d27a66..3a25184e24 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -189,7 +189,7 @@ controller_interface::return_type JointTrajectoryController::update( state_current_.time_from_start.set__sec(0); read_state_from_hardware(state_current_); - // currently carrying out a trajectory. + // currently carrying out a trajectory if (has_active_trajectory()) { bool first_sample = false; @@ -214,12 +214,32 @@ controller_interface::return_type JointTrajectoryController::update( if (valid_point) { + const rclcpp::Time traj_start = traj_external_point_ptr_->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 + const rclcpp::Time segment_time_from_start = traj_start + start_segment_itr->time_from_start; + // time_difference is + // - negative until first point is reached + // - counting from zero to time_from_start of next point + double time_difference = time.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - double time_difference = 0.0; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + // have we reached the end, are not holding position, and is a timeout configured? + // Check independently of other tolerances + if ( + !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && + 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()); + } + // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) { @@ -246,12 +266,6 @@ controller_interface::return_type JointTrajectoryController::update( if (default_tolerances_.goal_time_tolerance != 0.0) { - // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - time_difference = time.seconds() - traj_end.seconds(); - if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; @@ -946,6 +960,26 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 00d4517308..650175bae6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -91,6 +91,14 @@ joint_trajectory_controller: default_value: true, description: "If false, the last velocity point has to be zero or the goal will be rejected", } + cmd_timeout: { + type: double, + default_value: 0.0, # seconds + description: "Timeout after which the input command is considered stale. + Timeout is counted from the end of the trajectory (the last point). + cmd_timeout must be greater than constraints.goal_time, otherwise ignored. + If zero, timeout is deactivated", + } gains: __map_joints: p: { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 69d14efa5f..7af355eae9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -617,6 +617,147 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) executor.cancel(); } +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, accept_correct_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + double cmd_timeout = 3.0; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(cmd_timeout, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief cmd_timeout must be greater than constraints.goal_time + */ +TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 1.0); + rclcpp::Parameter goal_time_parameter("constraints.goal_time", 2.0); + SetUpAndActivateTrajectoryController( + executor, {cmd_timeout_parameter, goal_time_parameter}, false); + + EXPECT_DOUBLE_EQ(0.0, traj_controller_->get_cmd_timeout()); +} + +/** + * @brief check if no timeout is triggered + */ +TEST_P(TrajectoryControllerTestParameterized, no_timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // zero is default value, just for demonstration + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + + // is the trajectory still active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should still hold the points from above + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + // value of velocities is different from above due to spline interpolation + EXPECT_GT(state_msg->reference.velocities[0], 0.0); + EXPECT_GT(state_msg->reference.velocities[1], 0.0); + EXPECT_GT(state_msg->reference.velocities[2], 0.0); + + executor.cancel(); +} + +/** + * @brief check if timeout is triggered + */ +TEST_P(TrajectoryControllerTestParameterized, timeout) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double cmd_timeout = 0.1; + rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); + subscribeToState(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // update until end of trajectory -> no timeout should have occurred + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3); + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should have the trajectory with three points + EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); + + // update until timeout should have happened + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + // after timeout, set_hold_position adds a new trajectory + // is a trajectory active? + EXPECT_TRUE(traj_controller_->has_active_traj()); + // should be not more than one point now (from hold position) + EXPECT_FALSE(traj_controller_->has_nontrivial_traj()); + // should hold last position with zero velocity + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(points.at(2)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are normalized if configured so */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index d222e90e9c..6bccff9a6b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -137,6 +137,13 @@ class TestableJointTrajectoryController return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; } + bool has_nontrivial_traj() + { + return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + } + + double get_cmd_timeout() { return cmd_timeout_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; From 32cf43ab64cc2349a62b17bef410d54f5629a1fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 16 Sep 2023 21:02:59 +0200 Subject: [PATCH 08/22] [Docs] Improve interface description of JTC (backport #770) --- joint_trajectory_controller/doc/userdoc.rst | 27 ++++++++++----------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index bec732037b..cdc734500a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -14,16 +14,24 @@ Waypoints consist of positions, and optionally velocities and accelerations. *Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ -Hardware interface type [#f1]_ +Hardware interface types ------------------------------- -Currently joints with position, velocity, acceleration, and effort interfaces are supported. The joints can have one or more command interfaces, where the following control laws are applied at the same time: +Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations: + +* ``position`` +* ``position``, ``velocity`` +* ``position``, ``velocity``, ``acceleration`` +* ``velocity`` +* ``effort`` + +This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time: * For command interfaces ``position``, the desired positions are simply forwarded to the joints, * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. -* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop (:ref:`parameters`). +* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`). -This leads to the the following allowed combinations of command and state interfaces: +This leads to the following allowed combinations of command and state interfaces: * With command interface ``position``, there are no restrictions for state interfaces. * With command interface ``velocity``: @@ -32,12 +40,9 @@ This leads to the the following allowed combinations of command and state interf * no restrictions otherwise. * With command interface ``effort``, state interfaces must include ``position, velocity``. -* With command interface ``acceleration``, there are no restrictions for state interfaces. Example controller configurations can be found :ref:`below `. -Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). - Other features -------------- @@ -119,14 +124,8 @@ States ,,,,,,,,,,,,,,,,,, The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. - -Legal combinations of state interfaces are: -* ``position`` -* ``position`` and ``velocity`` -* ``position``, ``velocity`` and ``acceleration`` -* ``effort`` +Legal combinations of state interfaces are given in section `Hardware Interface Types`_. Commands ,,,,,,,,, From c4d591650b1a47e69397bd803803039fc99b00dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Sep 2023 14:38:46 +0200 Subject: [PATCH 09/22] [JTC] Add tests for acceleration command interface (backport #752) --- .../test/test_trajectory_controller.cpp | 113 +++++++++++++----- 1 file changed, 86 insertions(+), 27 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7af355eae9..6356eaa5cc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -57,8 +58,6 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } - void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) @@ -1010,7 +1009,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) || (traj_controller_->has_velocity_state_interface() && traj_controller_->has_velocity_command_interface())) { - // don't check against a value, because spline intepolation might overshoot depending on + // don't check against a value, because spline interpolation might overshoot depending on // interface combinations EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); @@ -1028,50 +1027,82 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor); + std::vector points_positions = {1.0, 2.0, 3.0}; + std::vector jumble_map = {1, 2, 0}; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ - joint_names_[1], joint_names_[2], joint_names_[0]}; + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]], joint_names_[jumble_map[2]]}; + traj_msg.joint_names = jumbled_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); traj_msg.points[0].positions.resize(3); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 3.0; - traj_msg.points[0].positions[2] = 1.0; - - if (traj_controller_->has_velocity_command_interface()) + traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); + traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); + traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); + traj_msg.points[0].velocities.resize(3); + for (size_t i = 0; i < 3; i++) { - traj_msg.points[0].velocities.resize(3); - traj_msg.points[0].velocities[0] = -0.1; - traj_msg.points[0].velocities[1] = -0.1; - traj_msg.points[0].velocities[2] = -0.1; + // factor 2 comes from the linear interpolation in the spline trajectory for constant + // acceleration + traj_msg.points[0].velocities[i] = + 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; } + trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update for 0.25 seconds + // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, + // otherwise acceleration is zero from the spline trajectory) // TODO(destogl): Make this time a bit shorter to increase stability on the CI? // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25)); + updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points_positions.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { + // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling + // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with + // feedforward term only EXPECT_GT(0.0, joint_vel_[0]); EXPECT_GT(0.0, joint_vel_[1]); EXPECT_GT(0.0, joint_vel_[2]); } - // TODO(anyone): add here checks for acceleration commands + + if (traj_controller_->has_acceleration_command_interface()) + { + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); + } + else + { + // no velocity state interface: no acceleration from trajectory sampling + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // effort should be nonzero, because we use PID with feedforward term + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } } /** @@ -1102,9 +1133,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); traj_msg.points[0].velocities[0] = - copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); traj_msg.points[0].velocities[1] = - copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); trajectory_publisher_->publish(traj_msg); } @@ -1126,22 +1157,50 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the velocity // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } + if (traj_controller_->has_acceleration_command_interface()) + { + // estimate the sign of the acceleration + // joint rotates forward + if (traj_controller_->has_velocity_state_interface()) + { + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + } + else + { + // no velocity state interface: no acceleration from trajectory sampling + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + } + EXPECT_NEAR(0.0, joint_acc_[2], threshold) + << "Joint 3 acc should be 0.0 since it's not in the goal"; + } + if (traj_controller_->has_effort_command_interface()) { // estimate the sign of the effort // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); EXPECT_NEAR(0.0, joint_eff_[2], threshold) << "Joint 3 effort should be 0.0 since it's not in the goal"; } - // TODO(anyone): add here checks for acceleration commands executor.cancel(); } @@ -1383,7 +1442,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = - copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + std::copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); // Replaced trajectory is a mix of previous and current goal From 99092c3c66b38e8bb54e6f5b0e621d3d2a8bc143 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 30 Oct 2023 11:08:07 +0000 Subject: [PATCH 10/22] Cleanup comments and unnecessary checks (backport #803) --- .../joint_trajectory_controller.hpp | 6 ++---- .../src/joint_trajectory_controller.cpp | 18 +----------------- 2 files changed, 3 insertions(+), 21 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 522164776e..e30c0b87d2 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 @@ -70,15 +70,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JointTrajectoryController(); /** - * @brief command_interface_configuration This controller requires the position command - * interfaces for the controlled joints + * @brief command_interface_configuration */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** - * @brief command_interface_configuration This controller requires the position and velocity - * state interfaces for the controlled joints + * @brief command_interface_configuration */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 3a25184e24..753b8eed68 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -654,17 +654,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); - // Check if the DoF has changed - if ((dof_ > 0) && (params_.joints.size() != dof_)) - { - RCLCPP_ERROR( - logger, - "The JointTrajectoryController does not support restarting with a different number of DOF"); - // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we - // can continue - return CallbackReturn::FAILURE; - } - + // get degrees of freedom dof_ = params_.joints.size(); // TODO(destogl): why is this here? Add comment or move @@ -694,12 +684,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - // // Specialized, child controllers set interfaces before calling configure function. - // if (command_interface_types_.empty()) - // { - // command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); - // } - if (params_.command_interfaces.empty()) { RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); From f516ab40524383fecde0096ecb758a2878615931 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 16 Aug 2023 15:35:57 +0200 Subject: [PATCH 11/22] [JTC] Fix typos, implicit cast, const member functions (backport #748) --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 2 +- ...oint_trajectory_controller_parameters.yaml | 4 ++-- .../test/test_trajectory_controller.cpp | 6 ++--- .../test/test_trajectory_controller_utils.hpp | 24 +++++++++---------- 5 files changed, 19 insertions(+), 19 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 e30c0b87d2..f2ad4c946e 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 @@ -257,7 +257,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool reset(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool has_active_trajectory(); + bool has_active_trajectory() const; using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 753b8eed68..cf3748cf15 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1636,7 +1636,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } -bool JointTrajectoryController::has_active_trajectory() +bool JointTrajectoryController::has_active_trajectory() const { return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 650175bae6..62edaad6d3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -109,7 +109,7 @@ joint_trajectory_controller: i: { type: double, default_value: 0.0, - description: "Intigral gain for PID" + description: "Integral gain for PID" } d: { type: double, @@ -119,7 +119,7 @@ joint_trajectory_controller: i_clamp: { type: double, default_value: 0.0, - description: "Integrale clamp. Symmetrical in both positive and negative direction." + description: "Integral clamp. Symmetrical in both positive and negative direction." } ff_velocity_scale: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6356eaa5cc..ae906257fc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1827,9 +1827,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co // set command values to NaN for (size_t i = 0; i < 3; ++i) { - joint_pos_[i] = 3.1 + i; - joint_vel_[i] = 0.25 + i; - joint_acc_[i] = 0.02 + i / 10.0; + joint_pos_[i] = 3.1 + static_cast(i); + joint_vel_[i] = 0.25 + static_cast(i); + joint_acc_[i] = 0.02 + static_cast(i) / 10.0; } SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 6bccff9a6b..28977ccfb7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -108,31 +108,31 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } - trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; } - bool has_position_state_interface() { return has_position_state_interface_; } + bool has_position_state_interface() const { return has_position_state_interface_; } - bool has_velocity_state_interface() { return has_velocity_state_interface_; } + bool has_velocity_state_interface() const { return has_velocity_state_interface_; } - bool has_acceleration_state_interface() { return has_acceleration_state_interface_; } + bool has_acceleration_state_interface() const { return has_acceleration_state_interface_; } - bool has_position_command_interface() { return has_position_command_interface_; } + bool has_position_command_interface() const { return has_position_command_interface_; } - bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_velocity_command_interface() const { return has_velocity_command_interface_; } - bool has_acceleration_command_interface() { return has_acceleration_command_interface_; } + bool has_acceleration_command_interface() const { return has_acceleration_command_interface_; } - bool has_effort_command_interface() { return has_effort_command_interface_; } + bool has_effort_command_interface() const { return has_effort_command_interface_; } - bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } - bool is_open_loop() { return params_.open_loop_control; } + bool is_open_loop() const { return params_.open_loop_control; } - bool has_active_traj() { return has_active_trajectory(); } + bool has_active_traj() const { return has_active_trajectory(); } - bool has_trivial_traj() + bool has_trivial_traj() const { return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; } From 1932f6e599967096ac2efe4c748e90a629c1154d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Oct 2023 12:58:44 +0200 Subject: [PATCH 12/22] Update requirements of state interfaces (backport #798) --- joint_trajectory_controller/doc/userdoc.rst | 10 ++++++++-- .../validate_jtc_parameters.hpp | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index cdc734500a..b6c4c1ecf3 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -17,7 +17,7 @@ Waypoints consist of positions, and optionally velocities and accelerations. Hardware interface types ------------------------------- -Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations: +Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here `_) are supported in the following combinations as command interfaces: * ``position`` * ``position``, ``velocity`` @@ -37,10 +37,16 @@ This leads to the following allowed combinations of command and state interfaces * With command interface ``velocity``: * if command interface ``velocity`` is the only one, state interfaces must include ``position, velocity`` . - * no restrictions otherwise. * With command interface ``effort``, state interfaces must include ``position, velocity``. +* With command interface ``acceleration``, state interfaces must include ``position, velocity``. + +Further restrictions of state interfaces exist: + +* ``velocity`` state interface cannot be used if ``position`` interface is missing. +* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present." + Example controller configurations can be found :ref:`below `. Other features diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index 6c909e3278..0b22c7cdd9 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -42,7 +42,7 @@ tl::expected command_interface_type_combinations( { return tl::make_unexpected( "'velocity' command interface can be used either alone or 'position' " - "interface has to be present"); + "command interface has to be present"); } if ( @@ -52,7 +52,7 @@ tl::expected command_interface_type_combinations( { return tl::make_unexpected( "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); + "'position' command interfaces are present"); } if ( From ae13bf6c6b629e6f5cbb2aabb7efb2e0dc93bf5e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Nov 2023 09:59:52 +0000 Subject: [PATCH 13/22] Rename wraparound class variables (backport #834) --- .../joint_trajectory_controller.hpp | 4 ++-- .../src/joint_trajectory_controller.cpp | 8 +++---- .../test/test_trajectory_actions.cpp | 6 ++++- .../test/test_trajectory_controller.cpp | 23 ++++++++----------- .../test/test_trajectory_controller_utils.hpp | 20 +++++++++------- 5 files changed, 33 insertions(+), 28 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 f2ad4c946e..7313eaf3dd 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 @@ -166,8 +166,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; - // Configuration for every joint, if position error is normalized - std::vector normalize_joint_error_; + // Configuration for every joint, if position error is wrapped around + std::vector joints_angle_wraparound_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cf3748cf15..1bd9070e6b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -134,7 +134,7 @@ controller_interface::return_type JointTrajectoryController::update( const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - if (normalize_joint_error_[index]) + if (joints_angle_wraparound_[index]) { // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -pihas_effort_command_interface()) == false) + { + // can't succeed with effort cmd if + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + } } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index ae906257fc..76989180c5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -201,8 +201,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params); // send msg @@ -508,14 +507,13 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) // Floating-point value comparison threshold const double EPS = 1e-6; /** - * @brief check if position error of revolute joints are normalized if not configured so + * @brief check if position error of revolute joints are angle_wraparound if not configured so */ -TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); subscribeToState(); @@ -758,14 +756,13 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) } /** - * @brief check if position error of revolute joints are normalized if configured so + * @brief check if position error of revolute joints are angle_wraparound if configured so */ -TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); subscribeToState(); @@ -806,7 +803,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); - // is error.positions[2] normalized? + // is error.positions[2] angle_wraparound? EXPECT_NEAR( state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); EXPECT_NEAR( @@ -835,7 +832,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) EXPECT_NEAR( k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); - // is error of positions[2] normalized? + // is error of positions[2] angle_wraparound? EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], @@ -863,7 +860,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) EXPECT_NEAR( k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], k_p * allowed_delta); - // is error of positions[2] normalized? + // is error of positions[2] angle_wraparound? EXPECT_GT(0.0, joint_eff_[2]); EXPECT_NEAR( k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 28977ccfb7..d44dc9ea27 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -198,7 +198,7 @@ class TrajectoryControllerTest : public ::testing::Test } void SetPidParameters( - double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) + double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -211,27 +211,31 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); - const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); + const rclcpp::Parameter angle_wraparound( + prefix + ".angle_wraparound", angle_wraparound_default); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool normalize_error = false) + bool angle_wraparound = false) { SetUpTrajectoryController(executor); + // add this to simplify tests, can be overwritten by parameters + rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true); + traj_controller_->get_node()->set_parameter(nonzero_vel_parameter); + // set pid parameters before configure - SetPidParameters(k_p, ff, normalize_error); + SetPidParameters(k_p, ff, angle_wraparound); + + // set optional parameters for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); } - // ignore velocity tolerances for this test since they aren't committed in test_robot->write() - rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); - traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); traj_controller_->get_node()->configure(); From aab61c7b775843bb7795de906820359d152d6868 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Nov 2023 10:05:53 +0000 Subject: [PATCH 14/22] [JTC] Fix tests when state offset is used (backport #797) * Simplify initialization of states * Rename read_state_from_hardware method * Don't set state_desired in on_activate --------- Co-authored-by: Dr. Denis --- .../joint_trajectory_controller.hpp | 8 +- .../src/joint_trajectory_controller.cpp | 19 +-- .../test/test_trajectory_controller.cpp | 118 +++++++++++------- .../test/test_trajectory_controller_utils.hpp | 72 +++++++---- 4 files changed, 135 insertions(+), 82 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 7313eaf3dd..bf2ed0a614 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 @@ -265,8 +265,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_hardware(JointTrajectoryPoint & state); + void read_state_from_state_interfaces(JointTrajectoryPoint & state); + /** Assign values from the command interfaces as state. + * This is only possible if command AND state interfaces exist for the same type, + * therefore needs check for both. + * @param[out] state to be filled with values from command interfaces. + * @return true if all interfaces exists and contain non-NaN values, false otherwise. + */ bool read_state_from_command_interfaces(JointTrajectoryPoint & state); bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1bd9070e6b..42dd7d4ec5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -187,7 +187,7 @@ controller_interface::return_type JointTrajectoryController::update( // current state update state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); + read_state_from_state_interfaces(state_current_); // currently carrying out a trajectory if (has_active_trajectory()) @@ -409,7 +409,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) @@ -1022,20 +1022,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; last_state_publish_time_ = get_node()->now(); - // Initialize current state storage if hardware state has tracking offset - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading from commands if - // those are not nan + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); if (read_state_from_command_interfaces(state)) { state_current_ = state; - state_desired_ = state; last_commanded_state_ = state; } + else + { + // Initialize current state storage from hardware + read_state_from_state_interfaces(state_current_); + read_state_from_state_interfaces(last_commanded_state_); + } // Should the controller start by holding position at the beginning of active state? if (params_.start_with_holding) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 76989180c5..43592c5d44 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1767,21 +1767,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e #endif // Testing that values are read from state interfaces when hardware is started for the first -// time and hardware state has offset --> this is indicated by NaN values in state interfaces +// time and hardware state has offset --> this is indicated by NaN values in command interfaces TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); // set command values to NaN - for (size_t i = 0; i < 3; ++i) - { - joint_pos_[i] = std::numeric_limits::quiet_NaN(); - joint_vel_[i] = std::numeric_limits::quiet_NaN(); - joint_acc_[i] = std::numeric_limits::quiet_NaN(); - } - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + std::vector initial_pos_cmd{3, std::numeric_limits::quiet_NaN()}; + std::vector initial_vel_cmd{3, std::numeric_limits::quiet_NaN()}; + std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; + + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from state interfaces + // (command interface are NaN) auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1790,70 +1792,96 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - traj_controller_->has_velocity_command_interface()) + if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); } // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - traj_controller_->has_acceleration_command_interface()) + if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); } } executor.cancel(); } -// Testing that values are read from state interfaces when hardware is started after some values +// Testing that values are read from command interfaces when hardware is started after some values // are set on the hardware commands TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - // set command values to NaN + // set command values to arbitrary values + std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; for (size_t i = 0; i < 3; ++i) { - joint_pos_[i] = 3.1 + static_cast(i); - joint_vel_[i] = 0.25 + static_cast(i); - joint_acc_[i] = 0.02 + static_cast(i) / 10.0; + initial_pos_cmd.push_back(3.1 + static_cast(i)); + initial_vel_cmd.push_back(0.25 + static_cast(i)); + initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from command interfaces auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); for (size_t i = 0; i < 3; ++i) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - - // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - traj_controller_->has_velocity_command_interface()) + // check position + if (traj_controller_->has_position_command_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + if (traj_controller_->has_velocity_command_interface()) + { + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + if (traj_controller_->has_acceleration_command_interface()) + { + // should have set it to last position + velocity + acceleration command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + } + } + else + { + // should have set it to last position + velocity command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + } + } + else + { + // should have set it to last position command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + } } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - traj_controller_->has_acceleration_command_interface()) + else { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index d44dc9ea27..b3f65536cb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -220,7 +220,11 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool angle_wraparound = false) + bool angle_wraparound = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { SetUpTrajectoryController(executor); @@ -239,12 +243,17 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->get_node()->configure(); - ActivateTrajectoryController(separate_cmd_and_state_values); + ActivateTrajectoryController( + separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, + initial_eff_joints); } void ActivateTrajectoryController( bool separate_cmd_and_state_values = false, - const std::vector initial_pos_joints = INITIAL_POS_JOINTS) + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -280,14 +289,17 @@ class TrajectoryControllerTest : public ::testing::Test cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); cmd_interfaces.back().set_value(initial_pos_joints[i]); cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); + cmd_interfaces.back().set_value(initial_vel_joints[i]); cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]); + cmd_interfaces.back().set_value(initial_acc_joints[i]); cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]); - joint_state_pos_[i] = initial_pos_joints[i]; - joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; - joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + cmd_interfaces.back().set_value(initial_eff_joints[i]); + if (separate_cmd_and_state_values) + { + joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; + joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; + joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + } state_interfaces.emplace_back(pos_state_interfaces_.back()); state_interfaces.emplace_back(vel_state_interfaces_.back()); state_interfaces.emplace_back(acc_state_interfaces_.back()); @@ -519,27 +531,33 @@ class TrajectoryControllerTest : public ::testing::Test // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0])) - << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is " - << joint_vel_[0]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1])) - << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is " - << joint_vel_[1]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2])) - << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is " - << joint_vel_[2]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) + << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() + << ", velocity command is " << joint_vel_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) + << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() + << ", velocity command is " << joint_vel_[1]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) + << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() + << ", velocity command is " << joint_vel_[2]; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0])) - << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is " - << joint_eff_[0]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1])) - << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is " - << joint_eff_[1]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2])) - << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is " - << joint_eff_[2]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) + << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() + << ", effort command is " << joint_eff_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) + << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() + << ", effort command is " << joint_eff_[1]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) + << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() + << ", effort command is " << joint_eff_[2]; } } } From dc837feede4d6e8d32041d576aa710db0f24919c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Nov 2023 10:07:14 +0000 Subject: [PATCH 15/22] [JTC] Activate update of dynamic parameters (backport #761) --- .../joint_trajectory_controller.hpp | 2 + .../src/joint_trajectory_controller.cpp | 122 +++++++++++------- .../test/test_trajectory_controller.cpp | 36 ++++++ .../test/test_trajectory_controller_utils.hpp | 2 + 4 files changed, 117 insertions(+), 45 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 bf2ed0a614..3e9fed1fdd 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 @@ -281,6 +281,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr response); private: + void update_pids(); + bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 42dd7d4ec5..786c0bbaf6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -127,6 +127,17 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + // update dynamic parameters + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + // use_closed_loop_pid_adapter_ is updated in on_configure only + if (use_closed_loop_pid_adapter_) + { + update_pids(); + default_tolerances_ = get_segment_tolerances(params_); + } + } auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, int index, @@ -716,29 +727,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - - // TODO(destogl): remove this in ROS2 Iron - // Check deprecated style for "ff_velocity_scale" parameter definition. - if (gains.ff_velocity_scale == 0.0) - { - RCLCPP_WARN( - get_node()->get_logger(), - "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " - "Maybe you are using deprecated format 'ff_velocity_scale/'!"); - - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); - } - else - { - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } - } + update_pids(); } // Configure joint position error normalization from ROS parameters (angle_wraparound) @@ -822,8 +811,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.command_interfaces).c_str(), get_interface_list(params_.state_interfaces).c_str()); - default_tolerances_ = get_segment_tolerances(params_); - + // parse remaining parameters const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -944,32 +932,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); - if (params_.cmd_timeout > 0.0) - { - if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) - { - cmd_timeout_ = params_.cmd_timeout; - } - else - { - // deactivate timeout - RCLCPP_WARN( - logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", - params_.cmd_timeout, default_tolerances_.goal_time_tolerance); - cmd_timeout_ = 0.0; - } - } - else - { - cmd_timeout_ = 0.0; - } - return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { @@ -1045,6 +1022,28 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } rt_is_holding_.writeFromNonRT(true); + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + return CallbackReturn::SUCCESS; } @@ -1642,6 +1641,39 @@ bool JointTrajectoryController::has_active_trajectory() const return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::update_pids() +{ + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + if (pids_[i]) + { + // update PIDs with gains from ROS parameters + pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + else + { + // Init PIDs with gains from ROS parameters + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + // Check deprecated style for "ff_velocity_scale" parameter definition. + if (gains.ff_velocity_scale == 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " + "Maybe you are using deprecated format 'ff_velocity_scale/'!"); + + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + } + else + { + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + } +} + void JointTrajectoryController::init_hold_position_msg() { hold_position_msg_ptr_ = std::make_shared(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 43592c5d44..5812c7032b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -465,6 +465,42 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief check if dynamic parameters are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + auto pids = traj_controller_->get_pids(); + + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_EQ(pids.size(), 3); + auto gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, 0.0); + + double kp = 1.0; + SetPidParameters(kp); + updateController(); + + pids = traj_controller_->get_pids(); + EXPECT_EQ(pids.size(), 3); + gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, kp); + } + else + { + // nothing to check here, skip further test + EXPECT_EQ(pids.size(), 0); + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b3f65536cb..58ca34ce14 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -130,6 +130,8 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + std::vector get_pids() const { return pids_; } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const From 25ae7177ab3282677926257ead835091ba1d0a92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:17:24 +0100 Subject: [PATCH 16/22] [JTC] Remove unused home pose (backport #845) --- .../joint_trajectory_controller.hpp | 2 -- .../src/joint_trajectory_controller.cpp | 28 ------------------- 2 files changed, 30 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 3e9fed1fdd..b563450d7a 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 @@ -183,8 +183,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 786c0bbaf6..df6c289a49 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -977,22 +977,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); traj_msg_external_point_ptr_.writeFromNonRT( std::shared_ptr()); @@ -1085,8 +1070,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return CallbackReturn::SUCCESS; } @@ -1094,13 +1077,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { - // go home - if (traj_home_point_ptr_ != nullptr) - { - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_external_point_ptr_ = traj_home_point_ptr_; - } - return CallbackReturn::SUCCESS; } @@ -1127,11 +1103,7 @@ bool JointTrajectoryController::reset() } } - // iterator has no default value - // prev_traj_point_ptr_; traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); return true; } From d7ce09374d429064cf7789a236d3af36788f71d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 19 Nov 2023 20:18:46 +0100 Subject: [PATCH 17/22] [JTC] Fix dynamic reconfigure of tolerances (backport #849) * Fix dynamic reconfigure of tolerances * Fix static cast syntax --- .../src/joint_trajectory_controller.cpp | 5 +- .../test/test_trajectory_controller.cpp | 53 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 5 ++ 3 files changed, 61 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index df6c289a49..f78f86e6cf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -131,11 +131,12 @@ controller_interface::return_type JointTrajectoryController::update( if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - // use_closed_loop_pid_adapter_ is updated in on_configure only + default_tolerances_ = get_segment_tolerances(params_); + // update the PID gains + // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) { update_pids(); - default_tolerances_ = get_segment_tolerances(params_); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 5812c7032b..92f0cb5272 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -501,6 +501,59 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) executor.cancel(); } +/** + * @brief check if dynamic tolerances are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + + // test default parameters + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 0.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01); + } + } + + // change parameters, update and see what happens + std::vector new_tolerances{ + rclcpp::Parameter("constraints.goal_time", 1.0), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02), + rclcpp::Parameter("constraints.joint1.trajectory", 1.0), + rclcpp::Parameter("constraints.joint2.trajectory", 2.0), + rclcpp::Parameter("constraints.joint3.trajectory", 3.0), + rclcpp::Parameter("constraints.joint1.goal", 10.0), + rclcpp::Parameter("constraints.joint2.goal", 20.0), + rclcpp::Parameter("constraints.joint3.goal", 30.0)}; + for (const auto & param : new_tolerances) + { + traj_controller_->get_node()->set_parameter(param); + } + updateController(); + + { + auto tols = traj_controller_->get_tolerances(); + EXPECT_EQ(tols.goal_time_tolerance, 1.0); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast(i) + 1.0); + EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast(i) + 1.0)); + EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02); + } + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 58ca34ce14..51641814d2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -132,6 +132,11 @@ class TestableJointTrajectoryController std::vector get_pids() const { return pids_; } + joint_trajectory_controller::SegmentTolerances get_tolerances() const + { + return default_tolerances_; + } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const From 16bd330a4a4b4fc006a8f1b79edaee1cb9287c92 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 27 Nov 2023 12:01:18 +0000 Subject: [PATCH 18/22] [JTC] Improve update methods for tests (backport #858) --- .../test/test_trajectory_actions.cpp | 16 +- .../test/test_trajectory_controller.cpp | 967 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 143 ++- 3 files changed, 565 insertions(+), 561 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3b80abcb9b..ed13a24485 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -242,7 +242,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -298,7 +298,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -350,7 +350,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -409,7 +409,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -460,7 +460,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -509,7 +509,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -555,7 +555,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -603,7 +603,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position, // i.e., active but trivial trajectory (one point only) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 92f0cb5272..7d1859fc85 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -26,8 +26,6 @@ #include #include -#include "gmock/gmock.h" - #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" @@ -103,77 +101,20 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, joint_names_); } TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - // set command_joints parameter + // set command_joints parameter different than joint_names_ const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names_); } TEST_P(TrajectoryControllerTestParameterized, activate) @@ -368,6 +309,11 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; @@ -474,7 +420,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); auto pids = traj_controller_->get_pids(); if (traj_controller_->use_closed_loop_pid_adapter()) @@ -485,7 +431,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) double kp = 1.0; SetPidParameters(kp); - updateController(); + updateControllerAsync(); pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); @@ -510,7 +456,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); // test default parameters { @@ -538,7 +484,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { traj_controller_->get_node()->set_parameter(param); } - updateController(); + updateControllerAsync(); { auto tols = traj_controller_->get_tolerances(); @@ -565,7 +511,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup without start_with_holding being set, we expect no active trajectory ASSERT_FALSE(traj_controller_->has_active_traj()); @@ -583,7 +529,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup with start_with_holding being set, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup @@ -604,7 +550,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -620,84 +565,206 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->output.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are angle_wraparound if configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // is error.positions[2] angle_wraparound? + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_LT(0.0, state_msg->output.velocities[2]); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + if (traj_controller_->has_velocity_command_interface()) + { // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) { - // we expect u = k_p * (s_d-s) + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // no normalization of position error + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_LT(0.0, state_msg->output.effort[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); @@ -737,13 +804,13 @@ TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) /** * @brief check if no timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, no_timeout) { rclcpp::executors::MultiThreadedExecutor executor; // zero is default value, just for demonstration rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -759,28 +826,27 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); // is the trajectory still active? EXPECT_TRUE(traj_controller_->has_active_traj()); // should still hold the points from above EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); - EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + EXPECT_NEAR(state_reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference.positions[2], points.at(2).at(2), 1e-2); // value of velocities is different from above due to spline interpolation - EXPECT_GT(state_msg->reference.velocities[0], 0.0); - EXPECT_GT(state_msg->reference.velocities[1], 0.0); - EXPECT_GT(state_msg->reference.velocities[2], 0.0); + EXPECT_GT(state_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); executor.cancel(); } @@ -788,6 +854,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) /** * @brief check if timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, timeout) { rclcpp::executors::MultiThreadedExecutor executor; @@ -795,7 +862,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); double kp = 1.0; // activate feedback control for testing velocity/effort PID SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); - subscribeToState(); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -820,11 +886,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // update until timeout should have happened updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - // after timeout, set_hold_position adds a new trajectory // is a trajectory active? EXPECT_TRUE(traj_controller_->has_active_traj()); @@ -844,130 +905,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } -/** - * @brief check if position error of revolute joints are angle_wraparound if configured so - */ -TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) -{ - rclcpp::executors::MultiThreadedExecutor executor; - constexpr double k_p = 10.0; - std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); - subscribeToState(); - - size_t n_joints = joint_names_.size(); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; - // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); - - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - - const auto allowed_delta = 0.1; - - // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); - - // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); - - // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); - - // is error.positions[2] angle_wraparound? - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], - state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); - - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_vel_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - } - } - - if (traj_controller_->has_effort_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } - } - - executor.cancel(); -} - void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { rclcpp::Parameter state_publish_rate_param( @@ -1043,7 +980,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}, true); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -1059,35 +995,34 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); - EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); - EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + EXPECT_EQ(n_joints, state_reference.velocities.size()); + EXPECT_EQ(n_joints, state_feedback.velocities.size()); + EXPECT_EQ(n_joints, state_error.velocities.size()); } if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + EXPECT_EQ(n_joints, state_reference.accelerations.size()); + EXPECT_EQ(n_joints, state_feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_error.accelerations.size()); } // no change in state interface should happen if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + EXPECT_EQ(state_feedback.velocities, INITIAL_VEL_JOINTS); } // is the velocity error correct? if ( @@ -1097,9 +1032,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { // don't check against a value, because spline interpolation might overshoot depending on // interface combinations - EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); - EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); - EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + EXPECT_GE(state_error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_error.velocities[2], points_velocities[0][2]); } executor.cancel(); @@ -1115,6 +1050,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) SetUpAndActivateTrajectoryController(executor); std::vector points_positions = {1.0, 2.0, 3.0}; std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ @@ -1124,29 +1060,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(3); traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); traj_msg.points[0].velocities.resize(3); - for (size_t i = 0; i < 3; i++) + traj_msg.points[0].accelerations.resize(3); + + for (size_t dof = 0; dof < 3; dof++) { - // factor 2 comes from the linear interpolation in the spline trajectory for constant - // acceleration - traj_msg.points[0].velocities[i] = - 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, - // otherwise acceleration is zero from the spline trajectory) - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { @@ -1167,19 +1101,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_acceleration_command_interface()) { - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_GT(0.0, joint_acc_[0]); - EXPECT_GT(0.0, joint_acc_[1]); - EXPECT_GT(0.0, joint_acc_[2]); - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); } if (traj_controller_->has_effort_command_interface()) @@ -1205,37 +1129,42 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; + const double dt = 0.25; trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + std::vector jumble_map = {1, 0}; + std::vector partial_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(2); traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + traj_msg.points[0].accelerations.resize(2); + for (size_t dof = 0; dof < 2; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "Joint 3 command should be current position"; } @@ -1247,7 +1176,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) + EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -1255,24 +1184,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the acceleration // joint rotates forward - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) - << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " - << joint_acc_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) - << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " - << joint_acc_[1]; - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - } - EXPECT_NEAR(0.0, joint_acc_[2], threshold) + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) << "Joint 3 acc should be 0.0 since it's not in the goal"; } @@ -1284,7 +1204,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) << "Joint 3 effort should be 0.0 since it's not in the goal"; } @@ -1326,47 +1246,45 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe traj_controller_->wait_for_trajectory(executor); // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; } @@ -1507,8 +1425,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); - std::vector> points_old{{{2., 3., 4.}}}; std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; @@ -1523,8 +1439,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = @@ -1539,7 +1455,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities[1] = 0.0; expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } /** @@ -1549,7 +1466,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1563,7 +1479,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0] trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time @@ -1572,14 +1489,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; @@ -1592,19 +1509,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = - rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); // it should not have accepted the new goal but finish the old one expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) @@ -1612,7 +1530,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); RCLCPP_WARN( traj_controller_->get_node()->get_logger(), @@ -1637,7 +1554,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); @@ -1650,28 +1568,21 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } -// TODO(destogl) this test fails with errors -// second publish() gives an error, because end time is before current time -// as well as -// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, -// which exceeds COMMON_THRESHOLD, where -// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, -// 2: joint_pos_[0] evaluates to 6.2700020099999998, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_jump_when_state_tracking_error_updated/0, where GetParam() = -// ({ "position" }, { "position" }) (3372 ms) - -#if 0 TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1684,95 +1595,95 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; - publish(time_from_start, points, - rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, - rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance + EXPECT_NEAR( + joint_state_pos_[0] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + 0.1); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif - -// TODO(destogl) this test fails -// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, -// which exceeds COMMON_THRESHOLD, where -// 2: second_goal[0] evaluates to 6.5999999999999996, -// 2: joint_pos_[0] evaluates to 6.5670133649999993, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = -// ({ "position" }, { "position", "velocity" }) (3374 ms) -#if 0 + TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { rclcpp::executors::SingleThreadedExecutor executor; + // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1783,77 +1694,79 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; + double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / + (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from - // command (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in the goal direction from - // command (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from + // command (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in the goal direction from + // command (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif // Testing that values are read from state interfaces when hardware is started for the first // time and hardware state has offset --> this is indicated by NaN values in command interfaces @@ -2001,7 +1914,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -2033,7 +1946,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -2041,7 +1954,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point expectHoldingPoint(hold_position); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 51641814d2..bd27fd52a4 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -29,7 +29,7 @@ namespace { -const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 0.001; const double INITIAL_POS_JOINT1 = 1.1; const double INITIAL_POS_JOINT2 = 2.1; const double INITIAL_POS_JOINT3 = 3.1; @@ -151,6 +151,10 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -233,21 +237,24 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - SetUpTrajectoryController(executor); - - // add this to simplify tests, can be overwritten by parameters - rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true); - traj_controller_->get_node()->set_parameter(nonzero_vel_parameter); - - // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); - - // set optional parameters - for (const auto & param : parameters) + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), + [](const rclcpp::Parameter & param) { + return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; + }) != parameters.end(); + + std::vector parameters_local = parameters; + if (!has_nonzero_vel_param) { - traj_controller_->get_node()->set_parameter(param); + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local); + // set pid parameters before configure + SetPidParameters(k_p, ff, angle_wraparound); traj_controller_->get_node()->configure(); ActivateTrajectoryController( @@ -431,43 +438,85 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_publisher_->publish(traj_msg); } - void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; auto previous_time = start_time; - while (clock.now() < end_time) + while (clock.now() <= end_time) { auto now = clock.now(); traj_controller_->update(now, now - previous_time); previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); + } + } + + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + return end_time; } - void waitAndCompareState( + rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, - rclcpp::Duration controller_wait_time, double allowed_delta) + rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { { std::lock_guard guard(state_mutex_); state_msg_.reset(); } traj_controller_->wait_for_trajectory(executor); - updateController(controller_wait_time); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + auto end_time = updateControllerAsync(controller_wait_time, start_time); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta); } } @@ -477,10 +526,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocties and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR( - expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); + EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } std::shared_ptr getStateLegacy() const @@ -603,6 +653,47 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_joints( + std::vector state_joint_names, std::vector command_joint_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_joint_names.size() * state_interface_types_.size()); + for (const auto & joint : state_joint_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_joint_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), + command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + std::string controller_name_; std::vector joint_names_; From ca67f12fbb42e95fb8cfe49328fefc5e7709a840 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 30 Nov 2023 20:27:27 +0000 Subject: [PATCH 19/22] [JTC] Activate checks for parameter validation backport (#857) --- .../test/test_trajectory_controller.cpp | 157 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 22 ++- 2 files changed, 91 insertions(+), 88 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7d1859fc85..dc4e7d33a6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -122,8 +122,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - traj_controller_->get_node()->configure(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -133,8 +133,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) ASSERT_EQ( state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); - ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -194,13 +194,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); - auto state = traj_controller_->get_state(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ActivateTrajectoryController(); - - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -245,8 +242,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - ActivateTrajectoryController(false, deactivated_positions); - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(false, deactivated_positions); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // it should still be holding the position at time of deactivation @@ -2017,72 +2013,73 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); -// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` -// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -// { -// auto set_parameter_and_check_result = [&]() -// { -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// SetParameters(); // This call is replacing the way parameters are set via launch -// traj_controller_->get_node()->configure(); -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// }; -// -// SetUpTrajectoryController(false); -// -// // command interfaces: empty -// command_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // command interfaces: bad_name -// command_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // command interfaces: effort has to be only -// command_interface_types_ = {"effort", "position"}; -// set_parameter_and_check_result(); -// -// // command interfaces: velocity - position not present -// command_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // command interfaces: acceleration without position and velocity -// command_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: empty -// state_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // state interfaces: cannot not be effort -// state_interface_types_ = {"effort"}; -// set_parameter_and_check_result(); -// -// // state interfaces: bad name -// state_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // state interfaces: velocity - position not present -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: acceleration without position and velocity -// state_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // velocity-only command interface: position - velocity not present -// command_interface_types_ = {"velocity"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// -// // effort-only command interface: position - velocity not present -// command_interface_types_ = {"effort"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// } +/** + * @brief see if parameter validation is correct + * + * Note: generate_parameter_library validates parameters itself during on_init() method, but + * combinations of parameters are checked from JTC during on_configure() + */ +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ + // command interfaces: empty + command_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + auto state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + + // command interfaces: bad_name + command_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: effort has to be only + command_interface_types_ = {"effort", "position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: velocity - position not present + command_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: acceleration without position and velocity + command_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: empty + state_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: cannot not be effort + state_interface_types_ = {"effort"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: bad name + state_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: velocity - position not present + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + state_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: acceleration without position and velocity + state_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index bd27fd52a4..b40c17e4b2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -188,6 +188,17 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}) + { + auto ret = SetUpTrajectoryControllerLocal(parameters); + if (ret != controller_interface::return_type::OK) + { + FAIL(); + } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + } + + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}) { traj_controller_ = std::make_shared(); @@ -200,12 +211,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", node_options); - if (ret != controller_interface::return_type::OK) - { - FAIL(); - } - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + return traj_controller_->init(controller_name_, "", node_options); } void SetPidParameters( @@ -262,7 +268,7 @@ class TrajectoryControllerTest : public ::testing::Test initial_eff_joints); } - void ActivateTrajectoryController( + rclcpp_lifecycle::State ActivateTrajectoryController( bool separate_cmd_and_state_values = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, @@ -320,7 +326,7 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_node()->activate(); + return traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } From b0f2e4cd64fc3bada20fdd8be2b252dec4d72168 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 1 Dec 2023 15:51:27 +0000 Subject: [PATCH 20/22] [JTC] Remove start_with_holding option (backport #839) --- .../src/joint_trajectory_controller.cpp | 7 ++---- ...oint_trajectory_controller_parameters.yaml | 5 ---- .../test/test_trajectory_controller.cpp | 23 ++----------------- 3 files changed, 4 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f78f86e6cf..e97caabcd3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1001,11 +1001,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } - // Should the controller start by holding position at the beginning of active state? - if (params_.start_with_holding) - { - add_new_trajectory_msg(set_hold_position()); - } + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 62edaad6d3..6614692f7d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -81,11 +81,6 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } - start_with_holding: { - type: bool, - default_value: true, - description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", - } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: true, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index dc4e7d33a6..47dd81c8bc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -496,24 +496,6 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) executor.cancel(); } -/** - * @brief check if hold on startup is deactivated - */ -TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) -{ - rclcpp::executors::MultiThreadedExecutor executor; - - rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); - - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup without start_with_holding being set, we expect no active trajectory - ASSERT_FALSE(traj_controller_->has_active_traj()); - - executor.cancel(); -} - /** * @brief check if hold on startup */ @@ -521,12 +503,11 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) { rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup with start_with_holding being set, we expect an active trajectory: + // after startup, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; From ee5a9c992a3e4551d33679ff878271854c81b524 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 1 Dec 2023 15:51:45 +0000 Subject: [PATCH 21/22] [JTC] Continue with last trajectory-point on success (backport #842) --- joint_trajectory_controller/doc/userdoc.rst | 4 +- .../joint_trajectory_controller.hpp | 12 +- .../src/joint_trajectory_controller.cpp | 16 ++- .../test/test_trajectory_actions.cpp | 136 +++++++++++++----- .../test/test_trajectory_controller.cpp | 14 +- .../test/test_trajectory_controller_utils.hpp | 56 +++----- 6 files changed, 162 insertions(+), 76 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index b6c4c1ecf3..79919c7426 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -151,12 +151,14 @@ Actions [#f1]_ /follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller - The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. + Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. +The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances. + .. _Subscriber: Subscriber [#f1]_ 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 b563450d7a..034e4f0cb8 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 @@ -173,7 +173,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Timeout to consider commands old double cmd_timeout_; - // Are we holding position? + // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; @@ -248,9 +248,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_hold_position(); + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr set_success_trajectory_point(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e97caabcd3..1d11684b12 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -374,7 +374,7 @@ 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_hold_position()); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -1564,6 +1564,20 @@ JointTrajectoryController::set_hold_position() return hold_position_msg_ptr_; } +std::shared_ptr +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); + + return hold_position_msg_ptr_; +} + bool JointTrajectoryController::contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ed13a24485..87d557df1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double kp = 0.0) + bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); + SetUpAndActivateTrajectoryController( + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); @@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - point.positions = point_positions; points.push_back(point); @@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) // note: the action goal also is a trivial trajectory - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(point_positions); - } - else + expectCommandPoint(point_positions); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + std::vector point_velocities{1.0, 1.0, 1.0}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions = point_positions; + point.velocities = point_velocities; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(point_positions, point_velocities); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); - point1.positions.resize(joint_names_.size()); - point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); - point2.positions.resize(joint_names_.size()); - point2.positions = points_positions.at(1); points.push_back(point2); @@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(points_positions.at(1)); - } - else + expectCommandPoint(points_positions.at(1)); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions = points_positions.at(0); + point1.velocities = points_velocities.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions = points_positions.at(1); + point2.velocities = points_velocities.at(1); + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1), points_velocities.at(1)); } /** @@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(0)); + expectCommandPoint(points_positions.at(0)); } /** @@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + expectCommandPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) // it should be holding the last position, // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(cancelled_position); + expectCommandPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 47dd81c8bc..6a3b52fc4f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // it should still be holding the position at time of deactivation // i.e., active but trivial trajectory (one point only) traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - expectHoldingPoint(deactivated_positions); + expectCommandPoint(deactivated_positions); executor.cancel(); } @@ -511,7 +511,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(initial_positions); executor.cancel(); } @@ -871,12 +871,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // should hold last position with zero velocity if (traj_controller_->has_position_command_interface()) { - expectHoldingPoint(points.at(2)); + expectCommandPoint(points.at(2)); } else { // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + expectCommandPoint(INITIAL_POS_JOINTS); } executor.cancel(); @@ -1894,7 +1894,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); } TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) @@ -1926,14 +1926,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point - expectHoldingPoint(hold_position); + expectCommandPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b40c17e4b2..20a598f398 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -551,7 +551,8 @@ class TrajectoryControllerTest : public ::testing::Test } void test_state_publish_rate_target(int target_msg_count); - void expectHoldingPoint(std::vector point) + void expectCommandPoint( + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) @@ -561,16 +562,16 @@ class TrajectoryControllerTest : public ::testing::Test { if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); + EXPECT_EQ(velocity.at(0), joint_vel_[0]); + EXPECT_EQ(velocity.at(1), joint_vel_[1]); + EXPECT_EQ(velocity.at(2), joint_vel_[2]); } if (traj_controller_->has_acceleration_command_interface()) @@ -587,40 +588,29 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ(0.0, joint_eff_[2]); } } - else + else // traj_controller_->use_closed_loop_pid_adapter() == true { // velocity or effort PID? - // velocity setpoint is always zero -> feedforward term does not have an effect // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", velocity command is " << joint_vel_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", velocity command is " << joint_vel_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", velocity command is " << joint_vel_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", effort command is " << joint_eff_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", effort command is " << joint_eff_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", effort command is " << joint_eff_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } } } } From 41304df77705458a728dde6bb7b216abc8e1648b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 4 Dec 2023 20:24:27 +0000 Subject: [PATCH 22/22] Fix floating point comparison in JTC (backport #879) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix floating point comparison in JTC * Fix format --------- Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1d11684b12..0865c13716 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1467,10 +1467,11 @@ bool JointTrajectoryController::validate_trajectory_msg( { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) { - if (trajectory.points.back().velocities.at(i) != 0.) + if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits::epsilon()) { RCLCPP_ERROR( - get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f", + get_node()->get_logger(), + "Velocity of last trajectory point of joint %s is not zero: %.15f", trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); return false; }