Add AckermannDriveStamped control to steering library

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -103,13 +103,13 @@ 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_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));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.x));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.y));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.z));
+  auto msg = controller_->input_ref_twist_.get();
+  EXPECT_TRUE(std::isnan(msg.twist.linear.x));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.y));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.z));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.x));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.y));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.z));
 }
 
 TEST_F(AckermannSteeringControllerTest, update_success)
@@ -161,11 +161,11 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
   ASSERT_FALSE(controller_->is_in_chained_mode());
 
   // set command statically
-  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
-  msg->header.stamp = controller_->get_node()->now();
-  msg->twist.linear.x = 0.1;
-  msg->twist.angular.z = 0.2;
-  controller_->input_ref_twist_.writeFromNonRT(msg);
+  ControllerReferenceMsg msg;
+  msg.header.stamp = controller_->get_node()->now();
+  msg.twist.linear.x = 0.1;
+  msg.twist.angular.z = 0.2;
+  controller_->input_ref_twist_.set(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_optional().value(),
     1.4179821977774734, COMMON_THRESHOLD);
 
-  EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
+  EXPECT_FALSE(std::isnan(controller_->input_ref_twist_.get().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_optional().value(),
     1.4179821977774734, COMMON_THRESHOLD);
 
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
+  EXPECT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -89,13 +89,13 @@ 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_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));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.x));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.y));
-  EXPECT_TRUE(std::isnan((*msg)->twist.angular.z));
+  auto msg = controller_->input_ref_twist_.get();
+  EXPECT_TRUE(std::isnan(msg.twist.linear.x));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.y));
+  EXPECT_TRUE(std::isnan(msg.twist.linear.z));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.x));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.y));
+  EXPECT_TRUE(std::isnan(msg.twist.angular.z));
 }
 
 TEST_F(BicycleSteeringControllerTest, update_success)
@@ -147,11 +147,11 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
   ASSERT_FALSE(controller_->is_in_chained_mode());
 
   // set command statically
-  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
-  msg->header.stamp = controller_->get_node()->now();
-  msg->twist.linear.x = 0.1;
-  msg->twist.angular.z = 0.2;
-  controller_->input_ref_twist_.writeFromNonRT(msg);
+  ControllerReferenceMsg msg;
+  msg.header.stamp = controller_->get_node()->now();
+  msg.twist.linear.x = 0.1;
+  msg.twist.angular.z = 0.2;
+  controller_->input_ref_twist_.set(msg);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -164,7 +164,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
+  EXPECT_FALSE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {
@@ -197,7 +197,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
     controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x));
+  EXPECT_TRUE(std::isnan(controller_->input_ref_twist_.get().twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
   for (const auto & interface : controller_->reference_interfaces_)
   {

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -88,13 +88,13 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
   rclcpp::Subscription<ControllerSteeringReferenceMsg>::SharedPtr ref_subscriber_steering_ =
     nullptr;
-  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_twist_;
-  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerSteeringReferenceMsg>>
-    input_ref_steering_;
+  // the RT Box containing the command messages
+  realtime_tools::RealtimeThreadSafeBox<ControllerTwistReferenceMsg> input_ref_twist_;
+  realtime_tools::RealtimeThreadSafeBox<ControllerSteeringReferenceMsg> input_ref_steering_;
+  // save the last references in case of unable to get value from box
+  ControllerTwistReferenceMsg current_ref_twist_;
+  ControllerSteeringReferenceMsg current_ref_steering_;
   rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms
-
-  // Command subscribers and Controller State publisher
-  rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
   using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher<ControllerStateMsgOdom>;
   using ControllerStatePublisherTf = realtime_tools::RealtimePublisher<ControllerStateMsgTf>;
 

steering_controllers_library/src/steering_controllers_library.cpp

@@ -53,6 +53,15 @@ void reset_controller_reference_msg(
   msg->steering_angle = std::numeric_limits<double>::quiet_NaN();
 }
 
+void reset_controller_reference_msg(
+  ControllerSteeringReferenceMsg & msg,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
+{
+  msg.header.stamp = node->now();
+  msg.linear_velocity = std::numeric_limits<double>::quiet_NaN();
+  msg.steering_angle = std::numeric_limits<double>::quiet_NaN();
+}
+
 }  // namespace
 
 namespace steering_controllers_library
@@ -271,21 +280,17 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
       "~/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);
+    reset_controller_reference_msg(current_ref_twist_, get_node());
+    input_ref_twist_.set(current_ref_twist_);
   }
   else
   {
     ref_subscriber_steering_ = get_node()->create_subscription<ControllerSteeringReferenceMsg>(
       "~/reference", subscribers_qos,
       std::bind(
         &SteeringControllersLibrary::reference_callback_steering, this, std::placeholders::_1));
-    std::shared_ptr<ControllerSteeringReferenceMsg> msg =
-      std::make_shared<ControllerSteeringReferenceMsg>();
-    reset_controller_reference_msg(msg, get_node());
-    input_ref_steering_.writeFromNonRT(msg);
+    reset_controller_reference_msg(current_ref_steering_, get_node());
+    input_ref_steering_.set(current_ref_steering_);
   }
 
   try
@@ -383,7 +388,7 @@ void SteeringControllersLibrary::reference_callback_twist(
 
   if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
   {
-    input_ref_twist_.writeFromNonRT(msg);
+    input_ref_twist_.set(*msg);
   }
   else
   {
@@ -411,7 +416,7 @@ void SteeringControllersLibrary::reference_callback_steering(
 
   if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
   {
-    input_ref_steering_.writeFromNonRT(msg);
+    input_ref_steering_.set(*msg);
   }
   else
   {
@@ -498,14 +503,17 @@ bool SteeringControllersLibrary::on_set_chained_mode(bool /*chained_mode*/) { re
 controller_interface::CallbackReturn SteeringControllersLibrary::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  // Set default value in command
+  // Try to set default value in command.
+  // If this fails, then another command will be received soon anyways.
   if (params_.use_twist_input)
   {
-    reset_controller_reference_msg(*(input_ref_twist_.readFromRT()), get_node());
+    reset_controller_reference_msg(current_ref_twist_, get_node());
+    input_ref_twist_.try_set(current_ref_twist_);
   }
   else
   {
-    reset_controller_reference_msg(*(input_ref_steering_.readFromRT()), get_node());
+    reset_controller_reference_msg(current_ref_steering_, get_node());
+    input_ref_steering_.try_set(current_ref_steering_);
   }
 
   return controller_interface::CallbackReturn::SUCCESS;
@@ -533,20 +541,84 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f
 {
   if (params_.use_twist_input)
   {
-    auto current_ref = *(input_ref_twist_.readFromRT());
-    if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
+    auto current_ref_op = input_ref_twist_.try_get();
+    if (current_ref_op.has_value())
+    {
+      current_ref_twist_ = current_ref_op.value();
+    }
+
+    const auto age_of_last_command = time - current_ref_twist_.header.stamp;
+
+    // accept message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0))
     {
-      reference_interfaces_[0] = current_ref->twist.linear.x;
-      reference_interfaces_[1] = current_ref->twist.angular.z;
+      if (!std::isnan(current_ref_twist_.twist.linear.x) && !std::isnan(current_ref_twist_.twist.angular.z))
+      {
+        reference_interfaces_[0] = current_ref_twist_.twist.linear.x;
+        reference_interfaces_[1] = current_ref_twist_.twist.angular.z;
+
+        if (ref_timeout_ == rclcpp::Duration::from_seconds(0))
+        {
+          current_ref_twist_.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+          current_ref_twist_.twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+
+          input_ref_twist_.try_set(current_ref_twist_);
+        }
+      }
+    }
+    else
+    {
+      if (!std::isnan(current_ref_twist_.twist.linear.x) && !std::isnan(current_ref_twist_.twist.angular.z))
+      {
+        reference_interfaces_[0] = std::numeric_limits<double>::quiet_NaN();
+        reference_interfaces_[1] = std::numeric_limits<double>::quiet_NaN();
+
+        current_ref_twist_.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+        current_ref_twist_.twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+
+        input_ref_twist_.try_set(current_ref_twist_);
+      }
     }
   }
   else
   {
-    auto current_ref = *(input_ref_steering_.readFromRT());
-    if (!std::isnan(current_ref->linear_velocity) && !std::isnan(current_ref->steering_angle))
+    auto current_ref_op = input_ref_steering_.try_get();
+    if (current_ref_op.has_value())
+    {
+      current_ref_steering_ = current_ref_op.value();
+    }
+
+    const auto age_of_last_command = time - current_ref_steering_.header.stamp;
+
+    // accept message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0))
     {
-      reference_interfaces_[0] = current_ref->linear_velocity;
-      reference_interfaces_[1] = current_ref->steering_angle;
+      if (!std::isnan(current_ref_steering_.linear_velocity) && !std::isnan(current_ref_steering_.steering_angle))
+      {
+        reference_interfaces_[0] = current_ref_steering_.linear_velocity;
+        reference_interfaces_[1] = current_ref_steering_.steering_angle;
+
+        if (ref_timeout_ == rclcpp::Duration::from_seconds(0))
+        {
+          current_ref_steering_.linear_velocity = std::numeric_limits<double>::quiet_NaN();
+          current_ref_steering_.steering_angle = std::numeric_limits<double>::quiet_NaN();
+
+          input_ref_steering_.try_set(current_ref_steering_);
+        }
+      }
+    }
+    else
+    {
+      if (!std::isnan(current_ref_steering_.linear_velocity) && !std::isnan(current_ref_steering_.steering_angle))
+      {
+        reference_interfaces_[0] = std::numeric_limits<double>::quiet_NaN();
+        reference_interfaces_[1] = std::numeric_limits<double>::quiet_NaN();
+
+        current_ref_steering_.linear_velocity = std::numeric_limits<double>::quiet_NaN();
+        current_ref_steering_.steering_angle = std::numeric_limits<double>::quiet_NaN();
+
+        input_ref_steering_.try_set(current_ref_steering_);
+      }
     }
   }
 
@@ -567,8 +639,8 @@ 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 = params_.use_twist_input
-                                       ? time - (*(input_ref_twist_.readFromRT()))->header.stamp
-                                       : time - (*(input_ref_steering_.readFromRT()))->header.stamp;
+                                       ? time - current_ref_twist_.header.stamp
+                                       : time - current_ref_steering_.header.stamp;
 
     const auto timeout =
       age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0);

steering_controllers_library/src/steering_odometry.cpp

@@ -182,8 +182,7 @@ bool SteeringOdometry::update_from_velocity(
 
   double linear_velocity = get_linear_velocity_double_traction_axle(
     right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
-  const double angular_velocity =
-    convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_);
+  const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheel_base_;
 
   return update_odometry(linear_velocity, angular_velocity, dt);
 }