WIP: Add AckermannDriveStamped control to steering library

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -58,7 +58,7 @@ 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_command_, period.seconds(), params_.twist_input);
   }
   else
   {

bicycle_steering_controller/src/bicycle_steering_controller.cpp

@@ -56,7 +56,7 @@ 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_command_, period.seconds(), params_.twist_input);
   }
   else
   {

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -97,9 +97,10 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
 
   // Command subscribers and Controller State publisher
   rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
-  rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_ackermann_ = nullptr;
+  rclcpp::Subscription<ControllerAckermannReferenceMsg>::SharedPtr ref_subscriber_ackermann_ = nullptr;
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr ref_subscriber_unstamped_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerAckermannReferenceMsg>> input_ackermann_ref_;
   rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms
 
   using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher<ControllerStateMsgOdom>;
@@ -134,7 +135,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
 
   // last velocity commands for open loop odometry
   double last_linear_velocity_ = 0.0;
-  double last_angular_velocity_ = 0.0;
+  double last_angular_command_ = 0.0;
 
   std::vector<std::string> rear_wheels_state_names_;
   std::vector<std::string> front_wheels_state_names_;
@@ -144,6 +145,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback(
     const std::shared_ptr<ControllerTwistReferenceMsg> msg);
   void reference_callback_unstamped(const std::shared_ptr<geometry_msgs::msg::Twist> 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

@@ -133,7 +133,7 @@ class SteeringOdometry
    * \param omega_bz Angular velocity [rad/s]
    * \param dt      time difference to last call
    */
-  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
@@ -188,10 +188,11 @@ class SteeringOdometry
    * \param v_bx     Desired linear velocity of the robot in x_b-axis direction
    * \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 twist_input If true, then omega_bz will be interpreted as steering angle
    * \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 double v_bx, const double omega_bz, const bool open_loop = true, const bool twist_input=true);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -230,6 +231,13 @@ class SteeringOdometry
    */
   double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
+  /**
+   * \brief Calculates angular velocity from the desired steering angle
+   * \param v_bx     Linear velocity of the robot in x_b-axis direction
+   * \param phi Steering angle of the robot around x_z-axis
+   */
+  double convert_steering_angle_to_angular_velocity(const double v_bx, const 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

@@ -33,6 +33,9 @@ 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(
   const std::shared_ptr<ControllerTwistReferenceMsg> & msg,
@@ -49,6 +52,18 @@ void reset_controller_reference_msg(
 
 }  // namespace
 
+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 steering_controllers_library
 {
 SteeringControllersLibrary::SteeringControllersLibrary()
@@ -114,12 +129,20 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
 
   // Reference Subscriber
   ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);
-  if (params_.use_stamped_vel)
+  if (params_.twist_input && params_.use_stamped_vel)
   {
     ref_subscriber_twist_ = get_node()->create_subscription<ControllerTwistReferenceMsg>(
       "~/reference", subscribers_qos,
       std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1));
   }
+  else if (!params_.twist_input)
+  {
+    ref_subscriber_ackermann_ =
+      get_node()->create_subscription<ControllerAckermannReferenceMsg>(
+        "~/reference_ackermann", subscribers_qos,
+        std::bind(
+          &SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1));
+  }
   else
   {
     RCLCPP_WARN(
@@ -131,10 +154,20 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
         &SteeringControllersLibrary::reference_callback_unstamped, 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)
+  {
+    std::shared_ptr<ControllerTwistReferenceMsg> msg =
+      std::make_shared<ControllerTwistReferenceMsg>();
+    reset_controller_reference_msg(msg, get_node());
+    input_ref_.writeFromNonRT(msg);
+  }
+  else
+  {
+    std::shared_ptr<ControllerAckermannReferenceMsg> msg =
+      std::make_shared<ControllerAckermannReferenceMsg>();
+    reset_controller_reference_msg(msg, get_node());
+    input_ackermann_ref_.writeFromNonRT(msg);
+  }
 
   try
   {
@@ -244,6 +277,34 @@ void SteeringControllersLibrary::reference_callback(
   }
 }
 
+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_ackermann_ref_.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_unstamped(
   const std::shared_ptr<geometry_msgs::msg::Twist> msg)
 {
@@ -344,10 +405,21 @@ SteeringControllersLibrary::state_interface_configuration() const
   }
   else
   {
-    for (size_t i = 0; i < front_wheels_state_names_.size(); i++)
+    if(params_.use_front_wheel_feedback)
+    {
+      for (size_t i = 0; i < front_wheels_state_names_.size(); i++)
+      {
+        state_interfaces_config.names.push_back(
+          front_wheels_state_names_[i] + "/" + traction_wheels_feedback);
+      }
+    }
+    else 
+    {
+      for (size_t i = 0; i < front_wheels_state_names_.size(); i++)
     {
       state_interfaces_config.names.push_back(
-        front_wheels_state_names_[i] + "/" + traction_wheels_feedback);
+        rear_wheels_state_names_[i] + "/" + traction_wheels_feedback);
+    }
     }
 
     for (size_t i = 0; i < rear_wheels_state_names_.size(); i++)
@@ -389,7 +461,12 @@ 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_.readFromRT()), get_node());
+  }
+  else{
+    reset_controller_reference_msg(*(input_ackermann_ref_.readFromRT()), get_node());
+  }
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -407,26 +484,52 @@ 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());
-  const auto age_of_last_command = time - (current_ref)->header.stamp;
+  if(params_.twist_input){
+    auto current_ref = *(input_ref_.readFromRT());
+    const auto age_of_last_command = time - (current_ref)->header.stamp;
 
-  // send message only if there is no timeout
-  if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0))
-  {
-    if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
+    // send message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0))
+    {
+      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;
+      if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
+      {
+        reference_interfaces_[0] = 0.0;
+        reference_interfaces_[1] = 0.0;
+        current_ref->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
+        current_ref->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+      }
     }
   }
-  else
-  {
-    if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
+  else {
+    auto current_ref = *(input_ackermann_ref_.readFromRT());
+    const auto age_of_last_command = time - (current_ref)->header.stamp;
+
+    // send message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0))
     {
-      reference_interfaces_[0] = 0.0;
-      reference_interfaces_[1] = 0.0;
-      current_ref->twist.linear.x = std::numeric_limits<double>::quiet_NaN();
-      current_ref->twist.angular.z = std::numeric_limits<double>::quiet_NaN();
+      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;
+      }
+    }
+    else
+    {
+      if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle))
+      {
+        reference_interfaces_[0] = 0.0;
+        reference_interfaces_[1] = 0.0;
+        current_ref->drive.speed = std::numeric_limits<double>::quiet_NaN();
+        current_ref->drive.steering_angle = std::numeric_limits<double>::quiet_NaN();
+      }
     }
   }
 
@@ -447,10 +550,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
   {
     // store (for open loop odometry) and set commands
     last_linear_velocity_ = reference_interfaces_[0];
-    last_angular_velocity_ = reference_interfaces_[1];
-
+    last_angular_command_ = reference_interfaces_[1];
+    
     auto [traction_commands, steering_commands] =
-      odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop);
+      odometry_.get_commands(last_linear_velocity_, last_angular_command_, params_.open_loop, params_.twist_input);
     if (params_.front_steering)
     {
       for (size_t i = 0; i < params_.rear_wheels_names.size(); i++)

steering_controllers_library/src/steering_controllers_library.yaml

@@ -63,6 +63,13 @@ steering_controllers_library:
     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,
+  }
+
   velocity_rolling_window_size: {
     type: int,
     default_value: 10,
@@ -113,6 +120,13 @@ steering_controllers_library:
     read_only: false,
   }
 
+  use_front_wheel_feedback: {
+    type: bool,
+    default_value: false,
+    description: "Choice which wheels to use for feedback, if front_wheel_feedback is false then rear wheels are used for feedback",
+    read_only: false,
+  }
+
   use_stamped_vel: {
     type: bool,
     default_value: false,

steering_controllers_library/src/steering_odometry.cpp

@@ -181,11 +181,11 @@ bool SteeringOdometry::update_from_velocity(
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
-void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt)
+void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt, const bool twist_input)
 {
   /// Save last linear and angular velocity:
   linear_ = v_bx;
-  angular_ = omega_bz;
+  angular_ = twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz);
 
   /// Integrate odometry:
   integrate_fk(v_bx, omega_bz, dt);
@@ -214,8 +214,13 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome
   return std::isfinite(phi) ? phi : 0.0;
 }
 
+double SteeringOdometry::convert_steering_angle_to_angular_velocity(double v_bx, double phi)
+{
+  return std::tan(phi) * v_bx / wheelbase_;
+}
+
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  const double v_bx, const double omega_bz, const bool open_loop)
+  const double v_bx, const double omega_bz, const bool open_loop, const bool twist_input)
 {
   // desired wheel speed and steering angle of the middle of traction and steering axis
   double Ws, phi, phi_IK = steer_pos_;
@@ -233,8 +238,9 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     Ws = v_bx / (wheel_radius_ * std::cos(phi_IK));  // using the measured steering angle
   }
 #endif
-  // steering angle
-  phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
+  // interprete twist input as steering angle if twist_input is false
+  phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz): omega_bz;
+  phi_IK = phi;
   if (open_loop)
   {
     phi_IK = phi;

tricycle_steering_controller/src/tricycle_steering_controller.cpp

@@ -56,7 +56,7 @@ bool TricycleSteeringController::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_command_, period.seconds(), params_.twist_input);
   }
   else
   {