Make diff_drive_controller a ChainableControllerInterface

Author: arthurlovekin

diff_drive_controller/diff_drive_plugin.xml

@@ -1,5 +1,5 @@
 <library path="diff_drive_controller">
-  <class name="diff_drive_controller/DiffDriveController" type="diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ControllerInterface">
+  <class name="diff_drive_controller/DiffDriveController" type="diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ChainableControllerInterface">
   <description>
     The differential drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot.
   </description>

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -25,14 +25,14 @@
 #include <string>
 #include <vector>
 
-#include "controller_interface/controller_interface.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
 #include "diff_drive_controller/odometry.hpp"
 #include "diff_drive_controller/speed_limiter.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_box.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 
@@ -41,7 +41,7 @@
 
 namespace diff_drive_controller
 {
-class DiffDriveController : public controller_interface::ControllerInterface
+class DiffDriveController : public controller_interface::ChainableControllerInterface
 {
   using TwistStamped = geometry_msgs::msg::TwistStamped;
 
@@ -52,7 +52,11 @@ class DiffDriveController : public controller_interface::ControllerInterface
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  controller_interface::return_type update(
+  // Chainable controller replaces update() with the following two functions
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  controller_interface::return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   controller_interface::CallbackReturn on_init() override;
@@ -76,6 +80,10 @@ class DiffDriveController : public controller_interface::ControllerInterface
     const rclcpp_lifecycle::State & previous_state) override;
 
 protected:
+  bool on_set_chained_mode(bool chained_mode) override;
+
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
   struct WheelHandle
   {
     std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
@@ -103,7 +111,7 @@ class DiffDriveController : public controller_interface::ControllerInterface
   Odometry odometry_;
 
   // Timeout to consider cmd_vel commands old
-  std::chrono::milliseconds cmd_vel_timeout_{500};
+  rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5);
 
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
@@ -117,11 +125,9 @@ class DiffDriveController : public controller_interface::ControllerInterface
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
 
-  realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
-  std::shared_ptr<TwistStamped> last_command_msg_;
-
-  std::queue<TwistStamped> previous_commands_;  // last two commands
+  realtime_tools::RealtimeBuffer<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
 
+  std::queue<std::array<double, 2>> previous_two_commands_;
   // speed limiters
   SpeedLimiter limiter_linear_;
   SpeedLimiter limiter_angular_;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -36,6 +36,25 @@ constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
 constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
 }  // namespace
 
+namespace
+{  // utility
+
+// called from RT control loop
+void reset_controller_reference_msg(
+  const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
+{
+  msg->header.stamp = node->now();
+  msg->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.linear.y = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.linear.z = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.angular.x = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.angular.y = std::numeric_limits<double>::quiet_NaN();
+  msg->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+}
+
+}  // namespace
+
 namespace diff_drive_controller
 {
 using namespace std::chrono_literals;
@@ -46,11 +65,11 @@ using hardware_interface::HW_IF_VELOCITY;
 using lifecycle_msgs::msg::State;
 
 DiffDriveController::DiffDriveController()
-: controller_interface::ControllerInterface(),
+: controller_interface::ChainableControllerInterface(),
   // dummy limiter, will be created in on_configure
   // could be done with shared_ptr instead -> but will break ABI
-  limiter_angular_(std::numeric_limits<double>::quiet_NaN()),
-  limiter_linear_(std::numeric_limits<double>::quiet_NaN())
+  limiter_linear_(std::numeric_limits<double>::quiet_NaN()),
+  limiter_angular_(std::numeric_limits<double>::quiet_NaN())
 {
 }
 
@@ -104,8 +123,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons
   return {interface_configuration_type::INDIVIDUAL, conf_names};
 }
 
-controller_interface::return_type DiffDriveController::update(
-  const rclcpp::Time & time, const rclcpp::Duration & period)
+controller_interface::return_type DiffDriveController::update_reference_from_subscribers(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto logger = get_node()->get_logger();
   if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
@@ -118,31 +137,60 @@ controller_interface::return_type DiffDriveController::update(
     return controller_interface::return_type::OK;
   }
 
-  // if the mutex is unable to lock, last_command_msg_ won't be updated
-  received_velocity_msg_ptr_.try_get([this](const std::shared_ptr<TwistStamped> & msg)
-                                     { last_command_msg_ = msg; });
+  const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
 
-  if (last_command_msg_ == nullptr)
+  if (command_msg_ptr == nullptr)
   {
     RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
     return controller_interface::return_type::ERROR;
   }
 
-  const auto age_of_last_command = time - last_command_msg_->header.stamp;
+  const auto age_of_last_command = time - command_msg_ptr->header.stamp;
   // Brake if cmd_vel has timeout, override the stored command
   if (age_of_last_command > cmd_vel_timeout_)
   {
-    last_command_msg_->twist.linear.x = 0.0;
-    last_command_msg_->twist.angular.z = 0.0;
+    reference_interfaces_[0] = 0.0;
+    reference_interfaces_[1] = 0.0;
+  }
+  else if (!std::isnan(command_msg_ptr->twist.linear.x) && !std::isnan(command_msg_ptr->twist.angular.z))
+  {
+    reference_interfaces_[0] = command_msg_ptr->twist.linear.x;
+    reference_interfaces_[1] = command_msg_ptr->twist.angular.z;
+  }
+  else 
+  {
+    RCLCPP_WARN(logger, "Command message contains NaNs. Not updating reference interfaces.");
+  }
+
+  previous_update_timestamp_ = time;
+
+  return controller_interface::return_type::OK;
+}
+
+controller_interface::return_type DiffDriveController::update_and_write_commands(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+  auto logger = get_node()->get_logger();
+  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
+  {
+    if (!is_halted)
+    {
+      halt();
+      is_halted = true;
+    }
+    return controller_interface::return_type::OK;
   }
 
   // command may be limited further by SpeedLimit,
   // without affecting the stored twist command
-  TwistStamped command = *last_command_msg_;
-  double & linear_command = command.twist.linear.x;
-  double & angular_command = command.twist.angular.z;
+  double linear_command = reference_interfaces_[0];
+  double angular_command = reference_interfaces_[1];
 
-  previous_update_timestamp_ = time;
+  if(std::isnan(linear_command) || std::isnan(angular_command))
+  {
+    // NaNs occur when the controller is first initialized and the reference interfaces are not yet set
+    return controller_interface::return_type::OK;
+  }
 
   // Apply (possibly new) multipliers:
   const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
@@ -239,22 +287,29 @@ controller_interface::return_type DiffDriveController::update(
     }
   }
 
-  auto & last_command = previous_commands_.back().twist;
-  auto & second_to_last_command = previous_commands_.front().twist;
+  double & last_linear = previous_two_commands_.back()[0];
+  double & second_to_last_linear = previous_two_commands_.front()[0];
+  double & last_angular = previous_two_commands_.back()[1];
+  double & second_to_last_angular = previous_two_commands_.front()[1];
+
   limiter_linear_.limit(
-    linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds());
+    linear_command, last_linear, second_to_last_linear, period.seconds());
   limiter_angular_.limit(
-    angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds());
-
-  previous_commands_.pop();
-  previous_commands_.emplace(command);
+    angular_command, last_angular, second_to_last_angular, period.seconds());
+  previous_two_commands_.pop();
+  previous_two_commands_.push({{linear_command, angular_command}});
 
   //    Publish limited velocity
   if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock())
   {
     auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
     limited_velocity_command.header.stamp = time;
-    limited_velocity_command.twist = command.twist;
+    limited_velocity_command.twist.linear.x = linear_command;
+    limited_velocity_command.twist.linear.y = 0.0;
+    limited_velocity_command.twist.linear.z = 0.0;
+    limited_velocity_command.twist.angular.x = 0.0;
+    limited_velocity_command.twist.angular.y = 0.0;
+    limited_velocity_command.twist.angular.z = angular_command;
     realtime_limited_velocity_publisher_->unlockAndPublish();
   }
 
@@ -301,7 +356,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius);
   odometry_.setVelocityRollingWindowSize(static_cast<size_t>(params_.velocity_rolling_window_size));
 
-  cmd_vel_timeout_ = std::chrono::milliseconds{static_cast<int>(params_.cmd_vel_timeout * 1000.0)};
+  cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout);
   publish_limited_velocity_ = params_.publish_limited_velocity;
 
   // TODO(christophfroehlich) remove deprecated parameters
@@ -394,12 +449,15 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
         limited_velocity_publisher_);
   }
 
-  last_command_msg_ = std::make_shared<TwistStamped>();
-  received_velocity_msg_ptr_.set([this](std::shared_ptr<TwistStamped> & stored_value)
-                                 { stored_value = last_command_msg_; });
-  // Fill last two commands with default constructed commands
-  previous_commands_.emplace(*last_command_msg_);
-  previous_commands_.emplace(*last_command_msg_);
+  const int nr_ref_itfs = 2;
+  reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits<double>::quiet_NaN());
+  previous_two_commands_.push({{0.0, 0.0}}); //needs zeros (not NaN) to catch early accelerations
+  previous_two_commands_.push({{0.0, 0.0}});
+
+  std::shared_ptr<TwistStamped> empty_msg_ptr = std::make_shared<TwistStamped>();
+  reset_controller_reference_msg(empty_msg_ptr, get_node());
+  received_velocity_msg_ptr_.reset();
+  received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr);
 
   // initialize command subscriber
   velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
@@ -419,8 +477,22 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
           "time, this message will only be shown once");
         msg->header.stamp = get_node()->get_clock()->now();
       }
-      received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
-                                     { stored_value = std::move(msg); });
+
+      const auto current_time_diff = get_node()->now() - msg->header.stamp;
+
+      if (cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || current_time_diff < cmd_vel_timeout_)
+      {
+        received_velocity_msg_ptr_.writeFromNonRT(msg);
+      }
+      else
+      {
+        RCLCPP_WARN(get_node()->get_logger(), 
+        "Ignoring the received message (timestamp %.10f) because it is older than "
+        "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)",
+        rclcpp::Time(msg->header.stamp).seconds(),
+        current_time_diff.seconds(),
+        cmd_vel_timeout_.seconds());
+      }
     });
 
   // initialize odometry publisher and message
@@ -498,6 +570,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
 controller_interface::CallbackReturn DiffDriveController::on_activate(
   const rclcpp_lifecycle::State &)
 {
+    // Set default value in command
+  reset_controller_reference_msg(*(received_velocity_msg_ptr_.readFromRT()), get_node());
+
   const auto left_result =
     configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_);
   const auto right_result =
@@ -546,6 +621,7 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup(
   {
     return controller_interface::CallbackReturn::ERROR;
   }
+  received_velocity_msg_ptr_.reset();
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -564,16 +640,16 @@ bool DiffDriveController::reset()
   odometry_.resetOdometry();
 
   // release the old queue
-  std::queue<TwistStamped> empty;
-  std::swap(previous_commands_, empty);
+  std::queue<std::array<double, 2>> empty;
+  std::swap(previous_two_commands_, empty);
 
   registered_left_wheel_handles_.clear();
   registered_right_wheel_handles_.clear();
 
   subscriber_is_active_ = false;
   velocity_command_subscriber_.reset();
 
-  received_velocity_msg_ptr_.set(nullptr);
+  received_velocity_msg_ptr_.reset();
   is_halted = false;
   return true;
 }
@@ -649,9 +725,35 @@ controller_interface::CallbackReturn DiffDriveController::configure_side(
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
+
+bool DiffDriveController::on_set_chained_mode(bool chained_mode)
+{
+  // Always accept switch to/from chained mode
+  return true || chained_mode;
+}
+
+std::vector<hardware_interface::CommandInterface>
+DiffDriveController::on_export_reference_interfaces()
+{
+  const int nr_ref_itfs = 2;
+  reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits<double>::quiet_NaN());
+  std::vector<hardware_interface::CommandInterface> reference_interfaces;
+  reference_interfaces.reserve(nr_ref_itfs);
+
+  reference_interfaces.push_back(hardware_interface::CommandInterface(
+    get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY,
+    &reference_interfaces_[0]));
+
+  reference_interfaces.push_back(hardware_interface::CommandInterface(
+    get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY,
+    &reference_interfaces_[1]));
+
+  return reference_interfaces;
+}
+
 }  // namespace diff_drive_controller
 
 #include "class_loader/register_macro.hpp"
 
 CLASS_LOADER_REGISTER_CLASS(
-  diff_drive_controller::DiffDriveController, controller_interface::ControllerInterface)
+  diff_drive_controller::DiffDriveController, controller_interface::ChainableControllerInterface)