Split SteeringOdometry into ForwardKinematics and InverseKinematics

steering_controllers_library/include/steering_controllers_library/forward_kinematics.hpp

@@ -0,0 +1,15 @@
+#ifndef FORWARD_KINEMATICS_HPP_
+#define FORWARD_KINEMATICS_HPP_
+
+#include "steering_controllers_library_parameters.hpp"
+
+class ForwardKinematics {
+public:
+    explicit ForwardKinematics(const steering_controllers_library::Params& params) : params_(params) {}
+    Odometry calculate(double linear_velocity, double angular_velocity);  // Implement forward calculation
+
+private:
+    steering_controllers_library::Params params_;
+};
+
+#endif  // FORWARD_KINEMATICS_HPP_

steering_controllers_library/include/steering_controllers_library/inverse_kinematics.hpp

@@ -0,0 +1,16 @@
+#ifndef INVERSE_KINEMATICS_HPP_
+#define INVERSE_KINEMATICS_HPP_
+
+#include "steering_controllers_library_parameters.hpp"
+#include <vector>
+
+class InverseKinematics {
+public:
+    explicit InverseKinematics(const steering_controllers_library::Params& params) : params_(params) {}
+    std::pair<std::vector<double>, std::vector<double>> calculateCommands(double linear_velocity, double angular_velocity);
+
+private:
+    steering_controllers_library::Params params_;
+};
+
+#endif  // INVERSE_KINEMATICS_HPP_

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -21,7 +21,9 @@
 #include <vector>
 
 #include "controller_interface/chainable_controller_interface.hpp"
+#include "forward_kinematics.hpp"
 #include "hardware_interface/handle.hpp"
+#include "inverse_kinematics.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.hpp"
 #include "realtime_tools/realtime_publisher.hpp"
@@ -125,6 +127,9 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
   std::vector<std::string> rear_wheels_state_names_;
   std::vector<std::string> front_wheels_state_names_;
 
+  ForwardKinematics forward_kinematics_;
+  InverseKinematics inverse_kinematics_;
+
 private:
   // callback for topic interface
   void reference_callback(const std::shared_ptr<ControllerTwistReferenceMsg> msg);

steering_controllers_library/src/forward_kinematics.cpp

@@ -0,0 +1,17 @@
+#include "forward_kinematics.hpp"
+
+struct Odometry {
+    double x;
+    double y;
+    double theta;
+};
+
+Odometry ForwardKinematics::calculate(double linear_velocity, double angular_velocity) {
+
+    Odometry odom_result;
+    odom_result.x = linear_velocity * params_.wheel_base;  
+    odom_result.y = 0.0;  
+    odom_result.theta = angular_velocity;  
+
+    return odom_result;
+}

steering_controllers_library/src/inverse_kinematics.cpp

@@ -0,0 +1,21 @@
+#include "inverse_kinematics.hpp"
+
+std::pair<std::vector<double>, std::vector<double>> InverseKinematics::calculateCommands(double linear_velocity, double angular_velocity) {
+    std::vector<double> traction_commands(params_.rear_wheels_names.size(), 0.0); 
+    std::vector<double> steering_commands(params_.front_wheels_names.size(), 0.0);
+
+    double wheel_base = params_.wheel_base;  
+    double track_width = params_.track_width;  
+
+    double radius = (angular_velocity != 0.0) ? (linear_velocity / angular_velocity) : std::numeric_limits<double>::infinity();
+
+    for (size_t i = 0; i < traction_commands.size(); ++i) {
+        traction_commands[i] = linear_velocity;  
+    }
+
+    for (size_t i = 0; i < steering_commands.size(); ++i) {
+        steering_commands[i] = std::atan2(wheel_base, radius);  
+    }
+
+    return std::make_pair(traction_commands, steering_commands);
+}

steering_controllers_library/src/steering_controllers_library.cpp

@@ -48,7 +48,9 @@ void reset_controller_reference_msg(
 namespace steering_controllers_library
 {
 SteeringControllersLibrary::SteeringControllersLibrary()
-: controller_interface::ChainableControllerInterface()
+: controller_interface::ChainableControllerInterface(),
+  forward_kinematics_(params_),
+  inverse_kinematics_(params_)
 {
 }
 
@@ -386,9 +388,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
     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);
+    auto [traction_commands, steering_commands] =
+      inverse_kinematics_.calculateCommands(last_linear_velocity_, last_angular_velocity_);
 
     if (params_.front_steering)
     {