diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index 9452339a81..f4903a5d32 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/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
   {
diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
index f7cdca092f..706f2b8364 100644
--- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
+++ b/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_)
   {
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index eb10e5ad00..bd990ea575 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp
+++ b/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
   {
diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
index 23103f7974..fe45002e3b 100644
--- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
+++ b/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_)
   {
diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt
index d57b00d278..74cb19dd03 100644
--- a/steering_controllers_library/CMakeLists.txt
+++ b/steering_controllers_library/CMakeLists.txt
@@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2
   tf2_msgs
   tf2_geometry_msgs
+  ackermann_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -55,6 +56,7 @@ target_link_libraries(steering_controllers_library PUBLIC
                       realtime_tools::realtime_tools
                       tf2::tf2
                       tf2_geometry_msgs::tf2_geometry_msgs
+                      ${ackermann_msgs_TARGETS}
                       ${tf2_msgs_TARGETS}
                       ${geometry_msgs_TARGETS}
                       ${control_msgs_TARGETS}
@@ -72,9 +74,14 @@ if(BUILD_TESTING)
   target_include_directories(test_steering_controllers_library PRIVATE include)
   target_link_libraries(test_steering_controllers_library steering_controllers_library)
 
+  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_add_gmock(test_steering_odometry test/test_steering_odometry.cpp)
   target_link_libraries(test_steering_odometry steering_controllers_library)
-
 endif()
 
 install(
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp
index e700767e4c..9f52fd8f3f 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp
+++ b/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"
@@ -31,6 +32,7 @@
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
 
 #include "steering_controllers_library/steering_controllers_library_parameters.hpp"
 #include "steering_controllers_library/steering_odometry.hpp"
@@ -70,6 +72,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped;
+  using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped;
   using ControllerStateMsgOdom = nav_msgs::msg::Odometry;
   using ControllerStateMsgTf = tf2_msgs::msg::TFMessage;
   using SteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus;
@@ -83,7 +86,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 +132,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
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
index 7b243e795c..0751765ade 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/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]
diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml
index 7981c31e55..f1721e2b97 100644
--- a/steering_controllers_library/package.xml
+++ b/steering_controllers_library/package.xml
@@ -40,6 +40,7 @@
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>ackermann_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>
diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp
index f34b3a78e0..75c9775c5a 100644
--- a/steering_controllers_library/src/steering_controllers_library.cpp
+++ b/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;
@@ -519,7 +578,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;
 }
@@ -537,12 +603,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;
@@ -560,7 +637,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);
 
@@ -570,7 +650,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++)
     {
diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml
index 64a4db21fa..3276dc7373 100644
--- a/steering_controllers_library/src/steering_controllers_library.yaml
+++ b/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,
+  }
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 5f480ca03d..52fc248ee1 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -126,7 +126,8 @@ bool SteeringOdometry::update_from_velocity(
 {
   steer_pos_ = steer_pos;
   double linear_velocity = traction_wheel_vel * wheel_radius_;
-  const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_;
+  const double angular_velocity =
+    convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos);
 
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
@@ -160,7 +161,8 @@ 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 = std::tan(steer_pos_) * linear_velocity / wheel_base_;
+  const double angular_velocity =
+    convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_);
 
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
@@ -180,16 +182,18 @@ 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 = steer_pos_ * linear_velocity / wheel_base_;
+  const double angular_velocity =
+    convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_);
 
   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);
@@ -231,9 +235,14 @@ 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 / wheel_base_;
+}
+
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
   const double v_bx, const double omega_bz, const bool open_loop,
-  const bool reduce_wheel_speed_until_steering_reached)
+  const bool reduce_wheel_speed_until_steering_reached, 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_;
@@ -243,7 +252,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   {
     // TODO(anyone) this would be only possible if traction is on the steering axis
     phi = omega_bz > 0 ? M_PI_2 : -M_PI_2;
-    Ws = abs(omega_bz) * wheelbase_ / wheel_radius_;
+    Ws = abs(omega_bz) * wheel_base_ / wheel_radius_;
   }
   else
   {
@@ -252,7 +261,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   }
 #endif
   // steering angle
-  phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
+  phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz) : omega_bz;
   if (open_loop)
   {
     phi_IK = phi;
diff --git a/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml b/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml
new file mode 100644
index 0000000000..cad7c030cf
--- /dev/null
+++ b/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml
@@ -0,0 +1,17 @@
+test_steering_controllers_library:
+  ros__parameters:
+
+    reference_timeout: 0.1
+    front_steering: true
+    open_loop: false
+    velocity_rolling_window_size: 10
+    position_feedback: false
+    twist_input: false
+    rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
+    front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
+
+    wheelbase: 3.24644
+    front_wheel_track: 2.12321
+    rear_wheel_track: 1.76868
+    front_wheels_radius: 0.45
+    rear_wheels_radius: 0.45
diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml
index 26e9330630..1b0247c9da 100644
--- a/steering_controllers_library/test/steering_controllers_library_params.yaml
+++ b/steering_controllers_library/test/steering_controllers_library_params.yaml
@@ -5,6 +5,7 @@ test_steering_controllers_library:
     open_loop: false
     velocity_rolling_window_size: 10
     position_feedback: false
+    twist_input: true
     traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint]
     steering_joints_names: [front_right_steering_joint, front_left_steering_joint]
 
diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp
index 26750777c9..a60ccbf070 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.cpp
+++ b/steering_controllers_library/test/test_steering_controllers_library.cpp
@@ -104,7 +104,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
   const double TEST_LINEAR_VELOCITY_Y = 0.0;
   const double TEST_ANGULAR_VELOCITY_Z = 0.3;
 
-  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  std::shared_ptr<ControllerTwistReferenceMsg> msg =
+    std::make_shared<ControllerTwistReferenceMsg>();
 
   // adjusting to achieve age_of_last_command > ref_timeout
   msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
@@ -115,17 +116,18 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
   msg->twist.angular.x = std::numeric_limits<double>::quiet_NaN();
   msg->twist.angular.y = std::numeric_limits<double>::quiet_NaN();
   msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z;
-  controller_->input_ref_.writeFromNonRT(msg);
+  controller_->input_ref_twist_.writeFromNonRT(msg);
 
-  const auto age_of_last_command =
-    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+  const auto age_of_last_command = controller_->get_node()->now() -
+                                   (*(controller_->input_ref_twist_.readFromNonRT()))->header.stamp;
 
   // case 1 position_feedback = false
   controller_->params_.position_feedback = false;
 
   // age_of_last_command > ref_timeout_
   ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
-  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    (*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
   ASSERT_EQ(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
@@ -136,8 +138,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
   {
     EXPECT_TRUE(std::isnan(interface));
   }
-  ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
-  ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z);
+  ASSERT_EQ(
+    (*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    (*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z);
 
   EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
   for (const auto & interface : controller_->reference_interfaces_)
@@ -165,11 +169,12 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
   msg->twist.angular.x = std::numeric_limits<double>::quiet_NaN();
   msg->twist.angular.y = std::numeric_limits<double>::quiet_NaN();
   msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z;
-  controller_->input_ref_.writeFromNonRT(msg);
+  controller_->input_ref_twist_.writeFromNonRT(msg);
 
   // age_of_last_command > ref_timeout_
   ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
-  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    (*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
   ASSERT_EQ(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
@@ -180,8 +185,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
   {
     EXPECT_TRUE(std::isnan(interface));
   }
-  ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
-  ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z);
+  ASSERT_EQ(
+    (*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    (*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z);
 
   EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
   for (const auto & interface : controller_->reference_interfaces_)
diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp
index 22d70fd7a5..0cabb075b5 100644
--- a/steering_controllers_library/test/test_steering_controllers_library.hpp
+++ b/steering_controllers_library/test/test_steering_controllers_library.hpp
@@ -33,8 +33,10 @@
 
 using ControllerStateMsg =
   steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg;
-using ControllerReferenceMsg =
+using ControllerTwistReferenceMsg =
   steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg;
+using ControllerAckermannReferenceMsg =
+  steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg;
 
 // NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only
 
@@ -87,7 +89,7 @@ class TestableSteeringControllersLibrary
   }
 
   /**
-   * @brief wait_for_command blocks until a new ControllerReferenceMsg is received.
+   * @brief wait_for_command blocks until a new ControllerTwistReferenceMsg is received.
    * Requires that the executor is not spinned elsewhere between the
    *  message publication and the call to this function.
    */
@@ -141,7 +143,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test
     controller_ = std::make_unique<CtrlType>();
 
     command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
-    command_publisher_ = command_publisher_node_->create_publisher<ControllerReferenceMsg>(
+    command_publisher_ = command_publisher_node_->create_publisher<ControllerTwistReferenceMsg>(
       "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS());
   }
 
@@ -286,7 +288,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test
 
     wait_for_topic(command_publisher_->get_topic_name());
 
-    ControllerReferenceMsg msg;
+    ControllerTwistReferenceMsg msg;
     msg.twist.linear.x = linear;
     msg.twist.angular.z = angular;
 
@@ -327,7 +329,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test
   // Test related parameters
   std::unique_ptr<CtrlType> controller_;
   rclcpp::Node::SharedPtr command_publisher_node_;
-  rclcpp::Publisher<ControllerReferenceMsg>::SharedPtr command_publisher_;
+  rclcpp::Publisher<ControllerTwistReferenceMsg>::SharedPtr command_publisher_;
 };
 
 #endif  // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_
diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp
new file mode 100644
index 0000000000..eb3c6339cd
--- /dev/null
+++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp
@@ -0,0 +1,215 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_steering_controllers_library_ackermann.hpp"
+
+class SteeringControllersLibraryTest
+: public SteeringControllersLibraryFixture<TestableSteeringControllersLibrary>
+{
+};
+
+// checking if all interfaces, command, state and reference are exported as expected
+TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)
+{
+  SetUpController();
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size());
+  EXPECT_EQ(
+    cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL],
+    rear_wheels_names_[0] + "/" + traction_interface_name_);
+  EXPECT_EQ(
+    cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL],
+    rear_wheels_names_[1] + "/" + traction_interface_name_);
+  EXPECT_EQ(
+    cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL],
+    front_wheels_names_[0] + "/" + steering_interface_name_);
+  EXPECT_EQ(
+    cmd_if_conf.names[CMD_STEER_LEFT_WHEEL],
+    front_wheels_names_[1] + "/" + steering_interface_name_);
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size());
+  EXPECT_EQ(
+    state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL],
+    controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_);
+  EXPECT_EQ(
+    state_if_conf.names[STATE_TRACTION_LEFT_WHEEL],
+    controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_);
+  EXPECT_EQ(
+    state_if_conf.names[STATE_STEER_RIGHT_WHEEL],
+    controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_);
+  EXPECT_EQ(
+    state_if_conf.names[STATE_STEER_LEFT_WHEEL],
+    controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_);
+  EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+
+  // check ref itfs
+  auto reference_interfaces = controller_->export_reference_interfaces();
+  ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
+  for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
+  {
+    const std::string ref_itf_prefix_name =
+      std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+  }
+
+  // explicitly check the names of the two reference interfaces
+  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
+  EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_POSITION);
+}
+
+// Tests controller update_reference_from_subscribers and
+// its two cases for position_feedback true/false behavior
+// when too old msg is sent i.e age_of_last_command > ref_timeout case
+TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
+{
+  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);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_FALSE(controller_->is_in_chained_mode());
+
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+
+  // set command statically
+  const float TEST_LINEAR_VELOCITY_X = static_cast<float>(1.5);
+  const float TEST_STEERING_ANGLE = static_cast<float>(0.575875);
+
+  std::shared_ptr<ControllerAckermannReferenceMsg> msg =
+    std::make_shared<ControllerAckermannReferenceMsg>();
+
+  // adjusting to achieve age_of_last_command > ref_timeout
+  msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
+                      rclcpp::Duration::from_seconds(0.1);
+  msg->drive.speed = TEST_LINEAR_VELOCITY_X;
+  msg->drive.steering_angle = TEST_STEERING_ANGLE;
+  msg->drive.steering_angle_velocity = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.acceleration = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.jerk = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_ackermann_.writeFromNonRT(msg);
+
+  const auto age_of_last_command =
+    controller_->get_node()->now() -
+    (*(controller_->input_ref_ackermann_.readFromNonRT()))->header.stamp;
+
+  // case 1 position_feedback = false
+  controller_->params_.position_feedback = false;
+
+  // age_of_last_command > ref_timeout_
+  ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
+  ASSERT_EQ(
+    (*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1]));
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+  ASSERT_EQ(
+    (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle,
+    TEST_STEERING_ANGLE);
+
+  EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+
+  // Wheel velocities should reset to 0
+  EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0);
+  EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0);
+
+  // Steer angles should not reset
+  EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6);
+  EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6);
+
+  // case 2 position_feedback = true
+  controller_->params_.position_feedback = true;
+
+  // adjusting to achieve age_of_last_command > ref_timeout
+  msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
+                      rclcpp::Duration::from_seconds(0.1);
+  msg->drive.speed = TEST_LINEAR_VELOCITY_X;
+  msg->drive.steering_angle = TEST_STEERING_ANGLE;
+  msg->drive.steering_angle_velocity = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.acceleration = std::numeric_limits<double>::quiet_NaN();
+  msg->drive.jerk = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_ackermann_.writeFromNonRT(msg);
+
+  // age_of_last_command > ref_timeout_
+  ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
+  ASSERT_EQ(
+    (*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1]));
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+  ASSERT_EQ(
+    (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
+  ASSERT_EQ(
+    (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle,
+    TEST_STEERING_ANGLE);
+
+  EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
+  for (const auto & interface : controller_->reference_interfaces_)
+  {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+
+  // Wheel velocities should reset to 0
+  EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0);
+  EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0);
+
+  // Steer angles should not reset
+  EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6);
+  EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}
diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp
new file mode 100644
index 0000000000..6f9d7dfdfd
--- /dev/null
+++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp
@@ -0,0 +1,344 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_
+#define TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_
+
+#include <gmock/gmock.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "steering_controllers_library/steering_controllers_library.hpp"
+
+using ControllerStateMsg =
+  steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg;
+using ControllerAckermannReferenceMsg =
+  steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg;
+
+// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only
+
+// name constants for state interfaces
+static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0;
+static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1;
+static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2;
+static constexpr size_t STATE_STEER_LEFT_WHEEL = 3;
+
+// name constants for command interfaces
+static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0;
+static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1;
+static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2;
+static constexpr size_t CMD_STEER_LEFT_WHEEL = 3;
+
+static constexpr size_t NR_STATE_ITFS = 4;
+static constexpr size_t NR_CMD_ITFS = 4;
+static constexpr size_t NR_REF_ITFS = 2;
+
+static constexpr double WHEELBASE_ = 3.24644;
+static constexpr double FRONT_WHEEL_TRACK_ = 2.12321;
+static constexpr double REAR_WHEEL_TRACK_ = 1.76868;
+static constexpr double FRONT_WHEELS_RADIUS_ = 0.45;
+static constexpr double REAR_WHEELS_RADIUS_ = 0.45;
+
+namespace
+{
+constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
+constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
+}  // namespace
+// namespace
+
+// subclassing and friending so we can access member variables
+class TestableSteeringControllersLibrary
+: public steering_controllers_library::SteeringControllersLibrary
+{
+  FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces);
+  FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout);
+
+public:
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override
+  {
+    return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state);
+  }
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override
+  {
+    auto ref_itfs = on_export_reference_interfaces();
+    return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state);
+  }
+
+  /**
+   * @brief wait_for_command blocks until a new ControllerAckermannReferenceMsg is received.
+   * Requires that the executor is not spinned elsewhere between the
+   *  message publication and the call to this function.
+   */
+  void wait_for_command(
+    rclcpp::Executor & executor,
+    const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
+  {
+    auto until = get_node()->get_clock()->now() + timeout;
+    while (get_node()->get_clock()->now() < until)
+    {
+      executor.spin_some();
+      std::this_thread::sleep_for(std::chrono::microseconds(10));
+    }
+  }
+
+  void wait_for_commands(
+    rclcpp::Executor & executor,
+    const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500})
+  {
+    wait_for_command(executor, timeout);
+  }
+
+  // implementing methods which are declared virtual in the steering_controllers_library.hpp
+  void initialize_implementation_parameter_listener()
+  {
+    param_listener_ = std::make_shared<steering_controllers_library::ParamListener>(get_node());
+  }
+
+  controller_interface::CallbackReturn configure_odometry()
+  {
+    set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
+    odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_);
+    odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
+
+    return controller_interface::CallbackReturn::SUCCESS;
+  }
+
+  bool update_odometry(const rclcpp::Duration & /*period*/) { return true; }
+};
+
+// We are using template class here for easier reuse of Fixture in specializations of controllers
+template <typename CtrlType>
+class SteeringControllersLibraryFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase() {}
+
+  void SetUp()
+  {
+    // initialize controller
+    controller_ = std::make_unique<CtrlType>();
+
+    command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
+    command_publisher_ = command_publisher_node_->create_publisher<ControllerAckermannReferenceMsg>(
+      "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS());
+  }
+
+  static void TearDownTestCase() {}
+
+  void TearDown() { controller_.reset(nullptr); }
+
+protected:
+  void SetUpController(const std::string controller_name = "test_steering_controllers_library")
+  {
+    ASSERT_EQ(
+      controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()),
+      controller_interface::return_type::OK);
+
+    if (position_feedback_ == true)
+    {
+      traction_interface_name_ = "position";
+    }
+    else
+    {
+      traction_interface_name_ = "velocity";
+    }
+
+    std::vector<hardware_interface::LoanedCommandInterface> command_ifs;
+    command_itfs_.reserve(joint_command_values_.size());
+    command_ifs.reserve(joint_command_values_.size());
+
+    command_itfs_.emplace_back(
+      hardware_interface::CommandInterface(
+        rear_wheels_names_[0], traction_interface_name_,
+        &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL]));
+    command_ifs.emplace_back(command_itfs_.back());
+
+    command_itfs_.emplace_back(
+      hardware_interface::CommandInterface(
+        rear_wheels_names_[1], steering_interface_name_,
+        &joint_command_values_[CMD_TRACTION_LEFT_WHEEL]));
+    command_ifs.emplace_back(command_itfs_.back());
+
+    command_itfs_.emplace_back(
+      hardware_interface::CommandInterface(
+        front_wheels_names_[0], steering_interface_name_,
+        &joint_command_values_[CMD_STEER_RIGHT_WHEEL]));
+    command_ifs.emplace_back(command_itfs_.back());
+
+    command_itfs_.emplace_back(
+      hardware_interface::CommandInterface(
+        front_wheels_names_[1], steering_interface_name_,
+        &joint_command_values_[CMD_STEER_LEFT_WHEEL]));
+    command_ifs.emplace_back(command_itfs_.back());
+
+    std::vector<hardware_interface::LoanedStateInterface> state_ifs;
+    state_itfs_.reserve(joint_state_values_.size());
+    state_ifs.reserve(joint_state_values_.size());
+
+    state_itfs_.emplace_back(
+      hardware_interface::StateInterface(
+        rear_wheels_names_[0], traction_interface_name_,
+        &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL]));
+    state_ifs.emplace_back(state_itfs_.back());
+
+    state_itfs_.emplace_back(
+      hardware_interface::StateInterface(
+        rear_wheels_names_[1], traction_interface_name_,
+        &joint_state_values_[STATE_TRACTION_LEFT_WHEEL]));
+    state_ifs.emplace_back(state_itfs_.back());
+
+    state_itfs_.emplace_back(
+      hardware_interface::StateInterface(
+        front_wheels_names_[0], steering_interface_name_,
+        &joint_state_values_[STATE_STEER_RIGHT_WHEEL]));
+    state_ifs.emplace_back(state_itfs_.back());
+
+    state_itfs_.emplace_back(
+      hardware_interface::StateInterface(
+        front_wheels_names_[1], steering_interface_name_,
+        &joint_state_values_[STATE_STEER_LEFT_WHEEL]));
+    state_ifs.emplace_back(state_itfs_.back());
+
+    controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
+  }
+
+  void subscribe_and_get_messages(ControllerStateMsg & msg)
+  {
+    // create a new subscriber
+    ControllerStateMsg::SharedPtr received_msg;
+    rclcpp::Node test_subscription_node("test_subscription_node");
+    auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; };
+    auto subscription = test_subscription_node.create_subscription<ControllerStateMsg>(
+      "/test_steering_controllers_library/controller_state", 10, subs_callback);
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(test_subscription_node.get_node_base_interface());
+
+    // call update to publish the test value
+    ASSERT_EQ(
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+      controller_interface::return_type::OK);
+    // call update to publish the test value
+    // since update doesn't guarantee a published message, republish until received
+    int max_sub_check_loop_count = 5;  // max number of tries for pub/sub loop
+    while (max_sub_check_loop_count--)
+    {
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01));
+      const auto timeout = std::chrono::milliseconds{5};
+      const auto until = test_subscription_node.get_clock()->now() + timeout;
+      while (!received_msg && test_subscription_node.get_clock()->now() < until)
+      {
+        executor.spin_some();
+        std::this_thread::sleep_for(std::chrono::microseconds(10));
+      }
+      // check if message has been received
+      if (received_msg.get())
+      {
+        break;
+      }
+    }
+    ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
+                                              "controller/broadcaster update loop";
+    ASSERT_TRUE(received_msg);
+
+    // take message from subscription
+    msg = *received_msg;
+  }
+
+  void publish_commands(
+    const float linear = static_cast<float>(0.1), const float angular = static_cast<float>(0.2))
+  {
+    auto wait_for_topic = [&](const auto topic_name)
+    {
+      size_t wait_count = 0;
+      while (command_publisher_node_->count_subscribers(topic_name) == 0)
+      {
+        if (wait_count >= 5)
+        {
+          auto error_msg =
+            std::string("publishing to ") + topic_name + " but no node subscribes to it";
+          throw std::runtime_error(error_msg);
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        ++wait_count;
+      }
+    };
+
+    wait_for_topic(command_publisher_->get_topic_name());
+
+    ControllerAckermannReferenceMsg msg;
+    msg.drive.speed = linear;
+    msg.drive.steering_angle = angular;
+
+    command_publisher_->publish(msg);
+  }
+
+protected:
+  // Controller-related parameters
+  double reference_timeout_ = 2.0;
+  bool front_steering_ = true;
+  bool open_loop_ = false;
+  unsigned int velocity_rolling_window_size_ = 10;
+  bool position_feedback_ = false;
+  std::vector<std::string> rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"};
+  std::vector<std::string> front_wheels_names_ = {
+    "front_right_steering_joint", "front_left_steering_joint"};
+  std::vector<std::string> joint_names_ = {
+    rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]};
+
+  std::vector<std::string> rear_wheels_preceeding_names_ = {
+    "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"};
+  std::vector<std::string> front_wheels_preceeding_names_ = {
+    "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"};
+  std::vector<std::string> preceeding_joint_names_ = {
+    rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1],
+    front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]};
+
+  double wheelbase_ = 3.24644;
+  double front_wheel_track_ = 2.12321;
+  double rear_wheel_track_ = 1.76868;
+  double front_wheels_radius_ = 0.45;
+  double rear_wheels_radius_ = 0.45;
+
+  std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
+  std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};
+
+  std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
+  std::string steering_interface_name_ = "position";
+  // defined in setup
+  std::string traction_interface_name_ = "";
+  std::string preceeding_prefix_ = "pid_controller";
+
+  std::vector<hardware_interface::StateInterface> state_itfs_;
+  std::vector<hardware_interface::CommandInterface> command_itfs_;
+
+  // Test related parameters
+  std::unique_ptr<CtrlType> controller_;
+  rclcpp::Node::SharedPtr command_publisher_node_;
+  rclcpp::Publisher<ControllerAckermannReferenceMsg>::SharedPtr command_publisher_;
+};
+
+#endif  // TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_
diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp
index a211ac8bf4..b33655779b 100644
--- a/steering_controllers_library/test/test_steering_odometry.cpp
+++ b/steering_controllers_library/test/test_steering_odometry.cpp
@@ -76,8 +76,22 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right)
   odom.update_open_loop(1., -1., 1.);
   EXPECT_DOUBLE_EQ(odom.get_linear(), 1.);
   EXPECT_DOUBLE_EQ(odom.get_angular(), -1.);
+
+  EXPECT_GT(odom.get_x(), 0);  // pos x
+  EXPECT_LT(odom.get_y(), 0);  // neg y, ie. right
+}
+
+TEST(TestSteeringOdometry, ackermann_odometry_openloop_ackermanndrive_angular_left)
+{
+  steering_odometry::SteeringOdometry odom(1);
+  odom.set_wheel_params(1., 2., 1.);
+  odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
+  odom.update_open_loop(1., 1., 1., false);
+  EXPECT_DOUBLE_EQ(odom.get_linear(), 1.);
+  double expected_angular = (1.0 / 2.0) * std::tan(1.0);
+  EXPECT_NEAR(odom.get_angular(), expected_angular, 1e-6);
   EXPECT_GT(odom.get_x(), 0);  // pos x
-  EXPECT_LT(odom.get_y(), 0);  // neg y ie. right
+  EXPECT_GT(odom.get_y(), 0);  // pos y, ie. left
 }
 
 TEST(TestSteeringOdometry, ackermann_IK_linear)
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index 2c919cbfd8..6e3809818f 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -82,7 +82,8 @@ 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_velocity_, period.seconds(), params_.twist_input);
   }
   else
   {
diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp
index 4f9b53e31f..e6b0c287c6 100644
--- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp
@@ -96,7 +96,7 @@ TEST_F(TricycleSteeringControllerTest, 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));
@@ -158,7 +158,7 @@ TEST_F(TricycleSteeringControllerTest, 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)),
@@ -174,7 +174,7 @@ TEST_F(TricycleSteeringControllerTest, 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_)
   {
@@ -211,7 +211,7 @@ TEST_F(TricycleSteeringControllerTest, 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_)
   {