Integrated more feedback

Author: wittenator

steering_controllers_library/doc/userdoc.rst

@@ -85,8 +85,14 @@ Subscribers
 
 Used when controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is activated (``twist_input == true``):
 - ``<controller_name>/reference``  [`geometry_msgs/msg/TwistStamped <twist_msg_>`_]
+In this configuration the controller uses :
+- **Linear Velocity (`linear`)**: Represents the linear speed of the robot (in meters per second, m/s).
+- **Angular Velocity (`angular`)**: Represents the angular speed of the robot (in meters per second, m/s).
 When the controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is not activated (``twist_input == false``):
 - ``<controller_name>/reference``  [`control_msgs/msg/SteeringControllerCommand <steering_controller_command_msg_>`_]
+In this configuration the controller uses :
+- **Linear Velocity (`speed`)**: Represents the linear velocity of the robot (in meters per second, m/s).
+- **Steering angle (`steering_angle`)**: Represents the angle of the imaginary, central steering wheel relative to the vehicle’s longitudinal axis. Specific angles for individual steering joints are computed internally based on the kinematic model of the robot. (in radians, rad)
 
 Publishers
 ,,,,,,,,,,,

steering_controllers_library/src/steering_controllers_library.cpp

@@ -44,15 +44,6 @@ void reset_controller_reference_msg(
   msg.twist.angular.z = std::numeric_limits<double>::quiet_NaN();
 }
 
-void reset_controller_reference_msg(
-  const std::shared_ptr<ControllerSteeringReferenceMsg> & msg,
-  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
-{
-  msg->header.stamp = node->now();
-  msg->linear_velocity = std::numeric_limits<double>::quiet_NaN();
-  msg->steering_angle = std::numeric_limits<double>::quiet_NaN();
-}
-
 void reset_controller_reference_msg(
   ControllerSteeringReferenceMsg & msg,
   const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
@@ -647,10 +638,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
     const auto timeout =
       age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0);
 
-    // store (for open loop odometry) and set commands
-    last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0];
-    last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1];
-
     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_.use_twist_input);

steering_controllers_library/src/steering_odometry.cpp

@@ -182,21 +182,24 @@ 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);
 }
 
 void SteeringOdometry::update_open_loop(
-  const double v_bx, const double omega_bz, const double dt, const bool use_twist_input)
+  const double v_bx, const double last_angular_command_, const double dt,
+  const bool use_twist_input)
 {
   /// Save last linear and angular velocity:
   linear_ = v_bx;
-  angular_ =
-    use_twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz);
+  angular_ = use_twist_input
+               ? last_angular_command_
+               : convert_steering_angle_to_angular_velocity(v_bx, last_angular_command_);
 
   /// Integrate odometry:
-  integrate_fk(v_bx, omega_bz, dt);
+  integrate_fk(v_bx, last_angular_command_, dt);
 }
 
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track)

steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright (c) 2023, FaSTTUBe - Formula Student Team TU Berlin
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright (c) 2025, FaSTTUBe - Formula Student Team TU Berlin
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -331,7 +331,6 @@ class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test
   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_;