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/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/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/). diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index f62320870c..0866aab8b8 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -62,11 +62,27 @@ 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. 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. @@ -77,6 +93,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/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 92f0f9629d..79919c7426 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -14,29 +14,40 @@ 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 as command interfaces: + +* ``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``: * 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``, there are no restrictions for state interfaces. -Example controller configurations can be found :ref:`below `. +* With command interface ``acceleration``, state interfaces must include ``position, velocity``. + +Further restrictions of state interfaces exist: -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). +* ``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 -------------- @@ -105,7 +116,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 @@ -120,14 +130,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 ,,,,,,,,, @@ -147,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]_ @@ -162,7 +168,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..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 @@ -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; @@ -168,11 +166,15 @@ 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_; + // Timeout to consider commands old + double cmd_timeout_; + // 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; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = @@ -180,13 +182,12 @@ 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; 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; @@ -205,6 +206,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); @@ -246,20 +248,39 @@ 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 - void set_hold_position(); + std::shared_ptr set_success_trajectory_point(); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool has_active_trajectory() const; + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( 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); @@ -268,9 +289,12 @@ 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); + 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/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 230d0198f7..3bd4873a31 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -130,15 +130,15 @@ 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 { 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/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 ( diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cff2918b75..0865c13716 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -127,6 +127,18 @@ 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(); + 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(); + } + } auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, int index, @@ -134,7 +146,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 -piget_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 @@ -183,39 +199,58 @@ 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 (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + 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) { + 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_point_active_ptr_)->end(); + 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) @@ -227,7 +262,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; } @@ -235,18 +271,13 @@ 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; 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_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; @@ -299,7 +330,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 +345,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,33 +369,51 @@ 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_success_trajectory_point()); } 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()); + } + 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 } } @@ -373,7 +421,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) @@ -570,17 +618,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; @@ -619,17 +666,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 @@ -659,12 +696,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."); @@ -697,33 +728,11 @@ 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) - normalize_joint_error_.resize(dof_); + joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); @@ -731,11 +740,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { // TODO(anyone): Remove deprecation warning in the end of 2023 RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!"); - normalize_joint_error_[i] = gains.normalize_error; + joints_angle_wraparound_[i] = gains.normalize_error; } else { - normalize_joint_error_[i] = gains.angle_wraparound; + joints_angle_wraparound_[i] = gains.angle_wraparound; } } @@ -803,8 +812,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); @@ -812,6 +820,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(), @@ -927,6 +939,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( 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) { @@ -957,43 +978,54 @@ 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()); 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 - 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_); + } + + // 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 + 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; } @@ -1001,7 +1033,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( 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 +1046,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,19 +1067,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; + traj_external_point_ptr_.reset(); + return CallbackReturn::SUCCESS; } 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_point_active_ptr_ = &traj_home_point_ptr_; - } - return CallbackReturn::SUCCESS; } @@ -1069,12 +1101,7 @@ 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(); return true; } @@ -1171,6 +1198,7 @@ void JointTrajectoryController::topic_callback( if (subscriber_is_active_) { add_new_trajectory_msg(msg); + rt_is_holding_.writeFromNonRT(false); } }; @@ -1205,17 +1233,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 +1251,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(); @@ -1230,6 +1261,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 @@ -1435,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; } @@ -1511,7 +1544,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 +1553,30 @@ void JointTrajectoryController::preempt_active_goal() } } -void JointTrajectoryController::set_hold_position() +std::shared_ptr +JointTrajectoryController::set_hold_position() +{ + // Command to stay at current position + hold_position_msg_ptr_->points[0].positions = state_current_.positions; + + // set flag, otherwise tolerances will be checked with holding position too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + +std::shared_ptr +JointTrajectoryController::set_success_trajectory_point() { - trajectory_msgs::msg::JointTrajectory empty_msg; - empty_msg.header.stamp = rclcpp::Time(0); + // 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); - auto traj_msg = std::make_shared(empty_msg); - add_new_trajectory_msg(traj_msg); + return hold_position_msg_ptr_; } bool JointTrajectoryController::contains_interface_type( @@ -1571,6 +1621,66 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } +bool JointTrajectoryController::has_active_trajectory() const +{ + return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); +} + +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(); + 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/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index bb0b45bb80..6614692f7d 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"]], } @@ -79,6 +86,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: { @@ -89,7 +104,7 @@ joint_trajectory_controller: i: { type: double, default_value: 0.0, - description: "Intigral gain for PID" + description: "Integral gain for PID" } d: { type: double, @@ -99,7 +114,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, @@ -126,7 +141,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], } 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/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 fdea551c30..87d557df1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,12 +68,12 @@ 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, double ff = 1.0) { setup_executor_ = true; SetUpAndActivateTrajectoryController( - executor_, true, parameters, separate_cmd_and_state_values); + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); @@ -219,20 +219,20 @@ 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; // 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,17 +243,59 @@ 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()) + // 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); +} + +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}; { - EXPECT_EQ(1.0, joint_pos_[0]); - EXPECT_EQ(2.0, joint_pos_[1]); - EXPECT_EQ(3.0, joint_pos_[2]); + 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 @@ -265,24 +307,17 @@ 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 +328,62 @@ 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()) + // 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)); +} + +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}}}; { - 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); + 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)); } /** @@ -318,15 +403,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 +422,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 + 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(0)); } /** @@ -370,24 +454,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 +481,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 + 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)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -417,29 +498,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 +532,12 @@ 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 - updateController(rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(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) + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -503,15 +580,12 @@ 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 - updateController(rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(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) + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -523,13 +597,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 +625,12 @@ 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 - updateController(rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(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) + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -601,19 +670,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 - updateController(rclcpp::Duration::from_seconds(0.01)); + // run an update + updateControllerAsync(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) + expectCommandPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) @@ -659,7 +723,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr // will be accepted despite nonzero last point EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + if ((traj_controller_->has_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 72cd2e8ca9..6a3b52fc4f 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 @@ -25,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" @@ -57,8 +56,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) @@ -104,79 +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; - SetUpTrajectoryController(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_); - 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); - 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) @@ -184,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( @@ -195,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(); } @@ -204,9 +142,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { rclcpp::executors::MultiThreadedExecutor executor; - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpAndActivateTrajectoryController(executor, true, params); + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -252,19 +189,15 @@ 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(); + 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]); @@ -285,44 +218,37 @@ 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); + 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_velocity_command_interface()) - { - EXPECT_LE(0.0, joint_vel_[0]); - EXPECT_LE(0.0, joint_vel_[1]); - EXPECT_LE(0.0, joint_vel_[2]); - } - - 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(); - state = traj_controller_->get_state(); + + state = ActivateTrajectoryController(false, deactivated_positions); 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)); + expectCommandPoint(deactivated_positions); executor.cancel(); } @@ -330,7 +256,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(); @@ -379,10 +305,15 @@ 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; - SetUpAndActivateTrajectoryController(executor, true, {}); + SetUpAndActivateTrajectoryController(executor, {}); subscribeToState(); updateController(); @@ -476,19 +407,126 @@ 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); + + updateControllerAsync(); + 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); + updateControllerAsync(); + + 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 dynamic tolerances are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateControllerAsync(); + + // 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); + } + updateControllerAsync(); + + { + 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 + */ +TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor, {}); + + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + // 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}; + expectCommandPoint(initial_positions); + + executor.cancel(); +} + // 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)}; - SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false); - subscribeToState(); + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); size_t n_joints = joint_names_.size(); @@ -504,100 +542,103 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) publish(time_from_start, points, 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); - - 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()) { - // 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]); - // 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 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); 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]), 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 + 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 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)}; - SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true); - subscribeToState(); + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); size_t n_joints = joint_names_.size(); @@ -613,44 +654,38 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) 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); - // is error.positions[2] normalized? - 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); + // 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_msg->error.positions[2], - state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + 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_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], 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()) @@ -660,16 +695,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { // 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); - // is error of positions[2] normalized? + 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] - 2 * M_PI), 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 { @@ -688,16 +723,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { // 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); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); 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? + 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_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); } else { @@ -712,12 +747,147 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_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 + */ +// 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); + + 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); + + updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); + + // 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_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_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_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); + + executor.cancel(); +} + +/** + * @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; + 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); + + // 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)); + + // 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()) + { + expectCommandPoint(points.at(2)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectCommandPoint(INITIAL_POS_JOINTS); + } + + executor.cancel(); +} + void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { 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(); }); @@ -786,8 +956,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}, true); - subscribeToState(); + SetUpAndActivateTrajectoryController(executor, {}, true); size_t n_joints = joint_names_.size(); @@ -803,35 +972,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 ( @@ -839,11 +1007,11 @@ 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]); - 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(); @@ -857,50 +1025,71 @@ 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}; + double dt = 0.25; { 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].time_from_start = rclcpp::Duration::from_seconds(dt); 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; + 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); + traj_msg.points[0].accelerations.resize(3); - if (traj_controller_->has_velocity_command_interface()) + for (size_t dof = 0; dof < 3; dof++) { - 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; + 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 for 0.25 seconds - // 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)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); 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()) + { + 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()) + { + // 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]); + } } /** @@ -912,42 +1101,47 @@ 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]; 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] = - 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); + 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"; } @@ -955,22 +1149,41 @@ 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_NEAR(0.0, joint_vel_[2], threshold) + 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], COMMON_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 + 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"; + } + 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_NEAR(0.0, joint_eff_[2], threshold) + 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], COMMON_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(); } @@ -984,7 +1197,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]; @@ -1010,47 +1223,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"; } @@ -1066,7 +1277,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; @@ -1127,7 +1338,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; @@ -1189,9 +1400,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}); - - subscribeToState(); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); std::vector> points_old{{{2., 3., 4.}}}; std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; @@ -1207,12 +1416,12 @@ 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] = - 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 @@ -1223,7 +1432,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); } /** @@ -1232,8 +1442,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {}); - subscribeToState(); + SetUpAndActivateTrajectoryController(executor, {}); // TODO(anyone): add expectations for velocities and accelerations std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1247,7 +1456,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 @@ -1255,16 +1465,15 @@ 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); + 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, true, {}); - subscribeToState(); + SetUpAndActivateTrajectoryController(executor, {}); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; @@ -1277,27 +1486,27 @@ 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) { rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters}); - subscribeToState(); + SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); RCLCPP_WARN( traj_controller_->get_node()->get_logger(), @@ -1322,7 +1531,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"); @@ -1335,28 +1545,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, true, {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}; @@ -1369,95 +1572,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, true, {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}; @@ -1468,94 +1671,98 @@ 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 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, true, {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(); @@ -1564,76 +1771,171 @@ 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 + i; - joint_vel_[i] = 0.25 + i; - joint_acc_[i] = 0.02 + 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, true, {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]); } } 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; + 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); + 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); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // it should have aborted and be holding now + expectCommandPoint(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, params, true, 1.0); + + // 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); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); + + // it should have aborted and be holding now + 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 + expectCommandPoint(hold_position); +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, @@ -1692,72 +1994,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 874a814a5d..20a598f398 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,14 +21,15 @@ #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" +#include "joint_trajectory_controller/trajectory.hpp" 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; @@ -37,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 @@ -101,27 +108,52 @@ 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() const { return has_velocity_state_interface_; } - bool has_velocity_state_interface() { return has_velocity_state_interface_; } + bool has_acceleration_state_interface() const { return has_acceleration_state_interface_; } - bool has_acceleration_state_interface() { return has_acceleration_state_interface_; } + bool has_position_command_interface() const { return has_position_command_interface_; } - bool has_position_command_interface() { return has_position_command_interface_; } + bool has_velocity_command_interface() const { return has_velocity_command_interface_; } - bool has_velocity_command_interface() { return has_velocity_command_interface_; } + bool has_acceleration_command_interface() const { return has_acceleration_command_interface_; } - bool has_acceleration_command_interface() { return has_acceleration_command_interface_; } + bool has_effort_command_interface() const { return has_effort_command_interface_; } - bool has_effort_command_interface() { return has_effort_command_interface_; } + bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; } - bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + bool is_open_loop() const { return params_.open_loop_control; } - bool is_open_loop() { return params_.open_loop_control; } + 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 + { + 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_; } + + 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_; }; @@ -154,36 +186,36 @@ 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 ret = SetUpTrajectoryControllerLocal(parameters); if (ret != controller_interface::return_type::OK) { FAIL(); } executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - SetParameters(); } - void SetParameters() + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}) { - 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}); + traj_controller_ = std::make_shared(); + + 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); + + return traj_controller_->init(controller_name_, "", node_options); } 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(); @@ -196,35 +228,52 @@ 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, 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) + 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, use_local_parameters); - - // set pid parameters before configure - SetPidParameters(k_p, ff, normalize_error); - 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); } - // 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); + // 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(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) + 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, + 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; @@ -258,23 +307,26 @@ 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.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()); } 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(); } @@ -392,43 +444,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()); } } - void waitAndCompareState( + /** + * @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; + } + + 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); } } @@ -438,10 +532,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 @@ -456,6 +551,145 @@ class TrajectoryControllerTest : public ::testing::Test } void test_state_publish_rate_target(int target_msg_count); + 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) + EXPECT_TRUE(traj_controller_->has_trivial_traj()); + + if (traj_controller_->use_closed_loop_pid_adapter() == false) + { + if (traj_controller_->has_position_command_interface()) + { + 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(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()) + { + 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]); + } + } + else // traj_controller_->use_closed_loop_pid_adapter() == true + { + // velocity or effort PID? + // --> set kp > 0.0 in test + if (traj_controller_->has_velocity_command_interface()) + { + 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()) + { + 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]; + } + } + } + } + + 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]); + } + } + + /** + * @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_;