Add AckermannDriveStamped control to steering library

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -116,7 +116,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_.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

@@ -66,7 +66,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_.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

@@ -64,6 +64,16 @@ if(BUILD_TESTING)
     controller_interface
     hardware_interface
   )
+  add_rostest_with_parameters_gmock(
+    test_steering_controllers_library_ackermann test/test_steering_controllers_library_ackermann.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_ackermann_params.yaml)
+  target_include_directories(test_steering_controllers_library_ackermann PRIVATE include)
+  target_link_libraries(test_steering_controllers_library_ackermann steering_controllers_library)
+  ament_target_dependencies(
+    test_steering_controllers_library_ackermann
+    controller_interface
+    hardware_interface
+  )
   ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp)
   target_link_libraries(test_steering_odometry steering_controllers_library)
 

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -22,6 +22,7 @@
 
 #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"
@@ -83,7 +84,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 +130,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 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 twist_input = true);
 
   /**
    * \brief Set odometry type
@@ -206,12 +208,13 @@ 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 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 twist_input = true);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -250,6 +253,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/src/steering_controllers_library.cpp

@@ -28,6 +28,8 @@ namespace
 
 using ControllerTwistReferenceMsg =
   steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg;
+using ControllerAckermannReferenceMsg =
+  steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg;
 
 // called from RT control loop
 void reset_controller_reference_msg(
@@ -43,6 +45,18 @@ void reset_controller_reference_msg(
   msg->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
 }
 
+void reset_controller_reference_msg(
+  const std::shared_ptr<ControllerAckermannReferenceMsg> & msg,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
+{
+  msg->header.stamp = node->now();
+  msg->drive.speed = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.acceleration = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.jerk = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.steering_angle = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.steering_angle_velocity = std::numeric_limits<double>::quiet_NaN();
+}
+
 }  // namespace
 
 namespace steering_controllers_library
@@ -329,14 +343,29 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
 
   // Reference Subscriber
   ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);
-  ref_subscriber_twist_ = get_node()->create_subscription<ControllerTwistReferenceMsg>(
-    "~/reference", subscribers_qos,
-    std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1));
 
-  std::shared_ptr<ControllerTwistReferenceMsg> msg =
-    std::make_shared<ControllerTwistReferenceMsg>();
-  reset_controller_reference_msg(msg, get_node());
-  input_ref_.writeFromNonRT(msg);
+  if (params_.twist_input)
+  {
+    ref_subscriber_twist_ = get_node()->create_subscription<ControllerTwistReferenceMsg>(
+      "~/reference", subscribers_qos,
+      std::bind(
+        &SteeringControllersLibrary::reference_callback_twist, this, std::placeholders::_1));
+    std::shared_ptr<ControllerTwistReferenceMsg> msg =
+      std::make_shared<ControllerTwistReferenceMsg>();
+    reset_controller_reference_msg(msg, get_node());
+    input_ref_twist_.writeFromNonRT(msg);
+  }
+  else
+  {
+    ref_subscriber_ackermann_ = get_node()->create_subscription<ControllerAckermannReferenceMsg>(
+      "~/reference", subscribers_qos,
+      std::bind(
+        &SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1));
+    std::shared_ptr<ControllerAckermannReferenceMsg> msg =
+      std::make_shared<ControllerAckermannReferenceMsg>();
+    reset_controller_reference_msg(msg, get_node());
+    input_ref_ackermann_.writeFromNonRT(msg);
+  }
 
   try
   {
@@ -418,7 +447,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
-void SteeringControllersLibrary::reference_callback(
+void SteeringControllersLibrary::reference_callback_twist(
   const std::shared_ptr<ControllerTwistReferenceMsg> msg)
 {
   // if no timestamp provided use current time for command timestamp
@@ -433,7 +462,35 @@ void SteeringControllersLibrary::reference_callback(
 
   if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
   {
-    input_ref_.writeFromNonRT(msg);
+    input_ref_twist_.writeFromNonRT(msg);
+  }
+  else
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Received message has timestamp %.10f older for %.10f which is more then allowed timeout "
+      "(%.4f).",
+      rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
+      ref_timeout_.seconds());
+  }
+}
+
+void SteeringControllersLibrary::reference_callback_ackermann(
+  const std::shared_ptr<ControllerAckermannReferenceMsg> msg)
+{
+  // if no timestamp provided use current time for command timestamp
+  if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u)
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Timestamp in header is missing, using current time as command timestamp.");
+    msg->header.stamp = get_node()->now();
+  }
+  const auto age_of_last_command = get_node()->now() - msg->header.stamp;
+
+  if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
+  {
+    input_ref_ackermann_.writeFromNonRT(msg);
   }
   else
   {
@@ -507,7 +564,9 @@ SteeringControllersLibrary::on_export_reference_interfaces()
 
   reference_interfaces.push_back(
     hardware_interface::CommandInterface(
-      get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY,
+      get_node()->get_name() + std::string("/angular"),
+      (params_.twist_input ? hardware_interface::HW_IF_VELOCITY
+                           : hardware_interface::HW_IF_POSITION),
       &reference_interfaces_[1]));
 
   return reference_interfaces;
@@ -523,7 +582,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
   // Set default value in command
-  reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node());
+  if (params_.twist_input)
+  {
+    reset_controller_reference_msg(*(input_ref_twist_.readFromRT()), get_node());
+  }
+  else
+  {
+    reset_controller_reference_msg(*(input_ref_ackermann_.readFromRT()), get_node());
+  }
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -541,12 +607,23 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate(
 controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
-  auto current_ref = *(input_ref_.readFromRT());
-
-  if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
+  if (params_.twist_input)
+  {
+    auto current_ref = *(input_ref_twist_.readFromRT());
+    if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
+    {
+      reference_interfaces_[0] = current_ref->twist.linear.x;
+      reference_interfaces_[1] = current_ref->twist.angular.z;
+    }
+  }
+  else
   {
-    reference_interfaces_[0] = current_ref->twist.linear.x;
-    reference_interfaces_[1] = current_ref->twist.angular.z;
+    auto current_ref = *(input_ref_ackermann_.readFromRT());
+    if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle))
+    {
+      reference_interfaces_[0] = current_ref->drive.speed;
+      reference_interfaces_[1] = current_ref->drive.steering_angle;
+    }
   }
 
   return controller_interface::return_type::OK;
@@ -564,7 +641,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
 
   if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
   {
-    const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp;
+    const auto age_of_last_command =
+      params_.twist_input ? time - (*(input_ref_twist_.readFromRT()))->header.stamp
+                          : time - (*(input_ref_ackermann_.readFromRT()))->header.stamp;
+
     const auto timeout =
       age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0);
 
@@ -574,7 +654,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
 
     auto [traction_commands, steering_commands] = odometry_.get_commands(
       reference_interfaces_[0], reference_interfaces_[1], params_.open_loop,
-      params_.reduce_wheel_speed_until_steering_reached);
+      params_.reduce_wheel_speed_until_steering_reached, params_.twist_input);
 
     for (size_t i = 0; i < params_.traction_joints_names.size(); i++)
     {

steering_controllers_library/src/steering_controllers_library.yaml

@@ -167,3 +167,10 @@ steering_controllers_library:
     position_feedback is true then ``HW_IF_POSITION`` is taken as interface type",
     read_only: false,
   }
+
+  twist_input: {
+    type: bool,
+    default_value: true,
+    description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.",
+    read_only: false,
+  }