Merge remote-tracking branch 'origin/master' into scaled_jtc

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -173,6 +173,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
@@ -211,8 +212,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
-  EXPECT_NEAR(
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
+  EXPECT_NEAR(
     controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
   EXPECT_NEAR(
@@ -261,6 +263,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
@@ -276,6 +279,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
 
   subscribe_and_get_messages(msg);
 
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
     msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
@@ -190,7 +190,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
@@ -237,22 +237,22 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
   EXPECT_EQ(msg.linear_velocity_command[0], 1.1);
   EXPECT_EQ(msg.steering_angle_command[0], 2.2);
 
-  publish_commands();
+  publish_commands(0.1, 0.2);
   ASSERT_TRUE(controller_->wait_for_commands(executor));
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   subscribe_and_get_messages(msg);
 
-  EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD);
+  EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD);
   EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD);
 }
 

doc/migration/Jazzy.rst

@@ -18,3 +18,4 @@ joint_trajectory_controller
 * Empty trajectory messages are discarded (`#902 <https://github.com/ros-controls/ros2_controllers/pull/902>`_).
 * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
 * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint.
+* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). Adaptions to the action goal might be necessary.

doc/release_notes/Jazzy.rst

@@ -33,6 +33,20 @@ joint_trajectory_controller
 * Action field ``error_string`` is now filled with meaningful strings (`#887 <https://github.com/ros-controls/ros2_controllers/pull/887>`_).
 * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
 * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). ``angle_wraparound`` parameter was completely removed.
+* Tolerances sent with the action goal are now processed and used for the action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). For details, see the `JointTolerance message <https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg>`_:
+
+  .. code-block:: markdown
+
+    The tolerances specify the amount the position, velocity, and
+    accelerations can vary from the setpoints.  For example, in the case
+    of trajectory control, when the actual position varies beyond
+    (desired position + position tolerance), the trajectory goal may
+    abort.
+
+    There are two special values for tolerances:
+     * 0 - The tolerance is unspecified and will remain at whatever the default is
+     * -1 - The tolerance is "erased".  If there was a default, the joint will be
+            allowed to move without restriction.
 
 pid_controller
 ************************

joint_trajectory_controller/CMakeLists.txt

@@ -61,6 +61,10 @@ if(BUILD_TESTING)
   target_link_libraries(test_trajectory joint_trajectory_controller)
   target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
 
+  ament_add_gmock(test_tolerances test/test_tolerances.cpp)
+  target_link_libraries(test_tolerances joint_trajectory_controller)
+  target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES)
+
   ament_add_gmock(test_trajectory_controller
     test/test_trajectory_controller.cpp)
   set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)

joint_trajectory_controller/doc/userdoc.rst

@@ -152,7 +152,21 @@ Actions  [#f1]_
 
 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.
+Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message <https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg>`_:
+
+.. code-block:: markdown
+
+  The tolerances specify the amount the position, velocity, and
+  accelerations can vary from the setpoints.  For example, in the case
+  of trajectory control, when the actual position varies beyond
+  (desired position + position tolerance), the trajectory goal may
+  abort.
+
+  There are two special values for tolerances:
+    * 0 - The tolerance is unspecified and will remain at whatever the default is
+    * -1 - The tolerance is "erased".  If there was a default, the joint will be
+          allowed to move without restriction.
+
 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.
 

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -258,7 +258,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     size_t joint_names_size, const std::vector<double> & vector_field,
     const std::string & string_for_vector_field, size_t i, bool allow_empty) const;
 
+  // the tolerances from the node parameter
   SegmentTolerances default_tolerances_;
+  // the tolerances used for the current goal
+  realtime_tools::RealtimeBuffer<SegmentTolerances> active_tolerances_;
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   void preempt_active_goal();