Split SteeringOdometry into ForwardKinematics and InverseKinematics

steering_controllers_library/include/steering_controllers_library/forward_kinematics.hpp

@@ -0,0 +1,33 @@
+// 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 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,34 @@
+// 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 INVERSE_KINEMATICS_HPP_
+#define INVERSE_KINEMATICS_HPP_
+
+#include <vector>
+#include "steering_controllers_library_parameters.hpp"
+
+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);