Merge branch 'master' into jtc/angle_wraparound_trajectory

joint_trajectory_controller/doc/userdoc.rst

@@ -17,7 +17,7 @@ Waypoints consist of positions, and optionally velocities and accelerations.
 Hardware interface types
 -------------------------------
 
-Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_) are supported in the following combinations:
+Currently, joints with hardware interface types ``position``, ``velocity``, ``acceleration``, and ``effort`` (defined `here <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_) are supported in the following combinations as command interfaces:
 
 * ``position``
 * ``position``, ``velocity``
@@ -37,10 +37,16 @@ This leads to the following allowed combinations of command and state interfaces
 * With command interface ``velocity``:
 
   * if command interface ``velocity`` is the only one, state interfaces must include  ``position, velocity`` .
-  * no restrictions otherwise.
 
 * With command interface ``effort``, state interfaces must include  ``position, velocity``.
 
+* With command interface ``acceleration``, state interfaces must include  ``position, velocity``.
+
+Further restrictions of state interfaces exist:
+
+* ``velocity`` state interface cannot be used if ``position`` interface  is missing.
+* ``acceleration`` state interface cannot be used if ``position`` and ``velocity`` interfaces are not present."
+
 Example controller configurations can be found :ref:`below <ROS 2 interface>`.
 
 Other features

joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp

@@ -41,7 +41,7 @@ tl::expected<void, std::string> 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 (
@@ -51,7 +51,7 @@ tl::expected<void, std::string> 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 (

ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py

@@ -46,8 +46,8 @@ def __init__(self):
             self.goals.append(float_goal)
 
         self.get_logger().info(
-            f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\
-              every {wait_sec_between_publish} s'
+            f"Publishing {len(goal_names)} goals on topic '{publish_topic}' "
+            f"every {wait_sec_between_publish} s'"
         )
 
         self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1)

ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py

@@ -129,8 +129,8 @@ def get_sub_param(sub_param):
         publish_topic = "/" + controller_name + "/" + "joint_trajectory"
 
         self.get_logger().info(
-            f'Publishing {len(goal_names)} goals on topic "{publish_topic}" every '
-            "{wait_sec_between_publish} s"
+            f"Publishing {len(goal_names)} goals on topic '{publish_topic}' every "
+            f"{wait_sec_between_publish} s"
         )
 
         self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1)

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -184,7 +184,8 @@ class SteeringOdometry
    * \param theta_dot Desired angular velocity [rad/s]
    * \return Tuple of velocity commands and steering commands
    */
-  std::tuple<std::vector<double>, std::vector<double>> get_commands(double Vx, double theta_dot);
+  std::tuple<std::vector<double>, std::vector<double>> get_commands(
+    const double Vx, const double theta_dot);
 
   /**
    *  \brief Reset poses, heading, and accumulators

steering_controllers_library/src/steering_controllers_library.cpp

@@ -443,10 +443,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
   if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
   {
     // store and set commands
-    const double linear_command = reference_interfaces_[0];
-    const double angular_command = reference_interfaces_[1];
+    last_linear_velocity_ = reference_interfaces_[0];
+    last_angular_velocity_ = reference_interfaces_[1];
+
     auto [traction_commands, steering_commands] =
-      odometry_.get_commands(linear_command, angular_command);
+      odometry_.get_commands(last_linear_velocity_, last_angular_velocity_);
     if (params_.front_steering)
     {
       for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)

steering_controllers_library/src/steering_odometry.cpp

@@ -210,7 +210,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  double Vx, double theta_dot)
+  const double Vx, const double theta_dot)
 {
   // desired velocity and steering angle of the middle of traction and steering axis
   double Ws, alpha;