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
   {

ackermann_steering_controller/test/ackermann_steering_controller_params.yaml

@@ -7,6 +7,7 @@ test_ackermann_steering_controller:
     velocity_rolling_window_size: 10
     position_feedback: false
     use_stamped_vel: true
+    twist_input: true
     rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
     front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
 

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -175,16 +175,62 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
 
   // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
+    controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
+    controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
     COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
     COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
+    COMMON_THRESHOLD);
+
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
+  EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+}
+
+TEST_F(AckermannSteeringControllerTest, test_update_logic_ackermann)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->set_chained_mode(false);
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("twist_input", false));
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_FALSE(controller_->is_in_chained_mode());
+
+  // set command statically
+  std::shared_ptr<ControllerAckermannReferenceMsg> msg = std::make_shared<ControllerAckermannReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now();
+  msg->drive.speed = 0.1;
+  msg->drive.steering_angle = 0.2;
+  controller_->input_ackermann_ref_.writeFromNonRT(msg);
+
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
+  EXPECT_NEAR(
+    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.1055,
+    COMMON_THRESHOLD);
+  EXPECT_NEAR(
+    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.0944,
+    COMMON_THRESHOLD);
+  EXPECT_NEAR(
+    controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 0.188,
+    COMMON_THRESHOLD);
+  EXPECT_NEAR(
+    controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 0.215,
     COMMON_THRESHOLD);
 
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
@@ -235,6 +281,8 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
   }
 }
 
+
+
 TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status)
 {
   SetUpController();

ackermann_steering_controller/test/test_ackermann_steering_controller.hpp

@@ -37,6 +37,8 @@ using ControllerStateMsg =
   steering_controllers_library::SteeringControllersLibrary::AckermanControllerState;
 using ControllerReferenceMsg =
   steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg;
+using ControllerAckermannReferenceMsg =
+  steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg;
 
 // name constants for state interfaces
 using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL;
@@ -68,6 +70,7 @@ class TestableAckermannSteeringController
   FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success);
   FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success);
   FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic);
+  FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_ackermann);
   FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chained);
   FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success);
   FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status);

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]