Add AckermannDriveStamped control to steering library

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -57,7 +57,8 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
 {
   if (params_.open_loop)
   {
-    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
+    odometry_.update_open_loop(
+      last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input);
   }
   else
   {

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -103,7 +103,7 @@ TEST_F(AckermannSteeringControllerTest, activate_success)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
   // check that the message is reset
-  auto msg = controller_->input_ref_.readFromNonRT();
+  auto msg = controller_->input_ref_twist_.readFromNonRT();
   EXPECT_TRUE(std::isnan((*msg)->twist.linear.x));
   EXPECT_TRUE(std::isnan((*msg)->twist.linear.y));
   EXPECT_TRUE(std::isnan((*msg)->twist.linear.z));
@@ -165,7 +165,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
   msg->header.stamp = controller_->get_node()->now();
   msg->twist.linear.x = 0.1;
   msg->twist.angular.z = 0.2;
-  controller_->input_ref_.writeFromNonRT(msg);
+  controller_->input_ref_twist_.writeFromNonRT(msg);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -185,7 +185,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
     controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -225,7 +225,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
     controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {

bicycle_steering_controller/src/bicycle_steering_controller.cpp

@@ -47,7 +47,8 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
 {
   if (params_.open_loop)
   {
-    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
+    odometry_.update_open_loop(
+      last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input);
   }
   else
   {

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -89,7 +89,7 @@ TEST_F(BicycleSteeringControllerTest, activate_success)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
   // check that the message is reset
-  auto msg = controller_->input_ref_.readFromNonRT();
+  auto msg = controller_->input_ref_twist_.readFromNonRT();
   EXPECT_TRUE(std::isnan((*msg)->twist.linear.x));
   EXPECT_TRUE(std::isnan((*msg)->twist.linear.y));
   EXPECT_TRUE(std::isnan((*msg)->twist.linear.z));
@@ -151,7 +151,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
   msg->header.stamp = controller_->get_node()->now();
   msg->twist.linear.x = 0.1;
   msg->twist.angular.z = 0.2;
-  controller_->input_ref_.writeFromNonRT(msg);
+  controller_->input_ref_twist_.writeFromNonRT(msg);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -163,7 +163,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -195,7 +195,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {

steering_controllers_library/CMakeLists.txt

@@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2
   tf2_msgs
   tf2_geometry_msgs
+  ackermann_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -55,6 +56,7 @@ target_link_libraries(steering_controllers_library PUBLIC
                       realtime_tools::realtime_tools
                       tf2::tf2
                       tf2_geometry_msgs::tf2_geometry_msgs
+                      ${ackermann_msgs_TARGETS}
                       ${tf2_msgs_TARGETS}
                       ${geometry_msgs_TARGETS}
                       ${control_msgs_TARGETS}
@@ -72,9 +74,14 @@ if(BUILD_TESTING)
   target_include_directories(test_steering_controllers_library PRIVATE include)
   target_link_libraries(test_steering_controllers_library steering_controllers_library)
 
+  add_rostest_with_parameters_gmock(
+    test_steering_controllers_library_ackermann_input test/test_steering_controllers_library_ackermann_input.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_ackermann_params.yaml)
+  target_include_directories(test_steering_controllers_library_ackermann_input PRIVATE include)
+  target_link_libraries(test_steering_controllers_library_ackermann_input steering_controllers_library)
+
   ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp)
   target_link_libraries(test_steering_odometry steering_controllers_library)
-
 endif()
 
 install(

steering_controllers_library/doc/userdoc.rst

@@ -21,7 +21,7 @@ For an introduction to mobile robot kinematics and the nomenclature used here, s
 Execution logic of the controller
 ----------------------------------
 
-The controller uses velocity input, i.e., stamped `twist messages <twist_msg_>`_ where linear ``x`` and angular ``z`` components are used.
+The controller uses velocity input, i.e., stamped `twist messages <twist_msg_>`_ where linear ``x`` and angular ``z`` components are used if ``twist_input == true``. If ``twist_input == false``, the controller uses `ackermann drive messages <ackermann_drive_stamped_msg_>`_ where linear ``speed`` and angular ``steering_angle`` components are used.
 Values in other components are ignored.
 
 In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position.
@@ -82,9 +82,10 @@ With the following state interfaces:
 Subscribers
 ,,,,,,,,,,,,
 
-Used when controller is not in chained mode (``in_chained_mode == false``).
-
+Used when controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is activated (``twist_input == true``):
 - ``<controller_name>/reference``  [`geometry_msgs/msg/TwistStamped <twist_msg_>`_]
+When the controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is not activated (``twist_input == false``):
+- ``<controller_name>/reference``  [`ackermann_msgs/msg/AckermannDriveStamped <ackermann_drive_stamped_msg_>`_]
 
 Publishers
 ,,,,,,,,,,,

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -22,11 +22,13 @@
 
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "hardware_interface/handle.hpp"
+#include "rclcpp/duration.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 
 // TODO(anyone): Replace with controller specific messages
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
 #include "control_msgs/msg/steering_controller_status.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -70,6 +72,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped;
+  using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped;
   using ControllerStateMsgOdom = nav_msgs::msg::Odometry;
   using ControllerStateMsgTf = tf2_msgs::msg::TFMessage;
   using SteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus;
@@ -83,7 +86,11 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
 
   // Command subscribers and Controller State publisher
   rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
-  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_;
+  rclcpp::Subscription<ControllerAckermannReferenceMsg>::SharedPtr ref_subscriber_ackermann_ =
+    nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_twist_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerAckermannReferenceMsg>>
+    input_ref_ackermann_;
   rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms
 
   using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher<ControllerStateMsgOdom>;
@@ -125,7 +132,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
 
 private:
   // callback for topic interface
-  void reference_callback(const std::shared_ptr<ControllerTwistReferenceMsg> msg);
+  void reference_callback_twist(const std::shared_ptr<ControllerTwistReferenceMsg> msg);
+  void reference_callback_ackermann(const std::shared_ptr<ControllerAckermannReferenceMsg> msg);
 };
 
 }  // namespace steering_controllers_library

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -136,8 +136,10 @@ class SteeringOdometry
    * \param v_bx  Linear velocity   [m/s]
    * \param omega_bz Angular velocity [rad/s]
    * \param dt      time difference to last call
+   * \param use_twist_input If true, the input is twist, otherwise it is steering angle
    */
-  void update_open_loop(const double v_bx, const double omega_bz, const double dt);
+  void update_open_loop(
+    const double v_bx, const double omega_bz, const double dt, const bool use_twist_input = true);
 
   /**
    * \brief Set odometry type
@@ -206,12 +208,14 @@ class SteeringOdometry
    * \param omega_bz Desired angular velocity of the robot around x_z-axis
    * \param open_loop If false, the IK will be calculated using measured steering angle
    * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle
+   * \param use_twist_input If true, the input is twist, otherwise it is steering angle
    * has been reached
    * \return Tuple of velocity commands and steering commands
    */
   std::tuple<std::vector<double>, std::vector<double>> get_commands(
     const double v_bx, const double omega_bz, const bool open_loop = true,
-    const bool reduce_wheel_speed_until_steering_reached = false);
+    const bool reduce_wheel_speed_until_steering_reached = false,
+    const bool use_twist_input = true);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -250,6 +254,13 @@ class SteeringOdometry
    */
   double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
+  /**
+   * \brief Calculates angular velocity from the steering angle and linear velocity
+   * \param v_bx Linear velocity of the robot in x_b-axis direction
+   * \param phi Steering angle
+   */
+  double convert_steering_angle_to_angular_velocity(double v_bx, double phi);
+
   /**
    * \brief Calculates linear velocity of a robot with double traction axle
    * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]

steering_controllers_library/package.xml

@@ -40,6 +40,7 @@
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>ackermann_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>