From cd29184ca2b6f72605188b34a3e70f548502001e Mon Sep 17 00:00:00 2001 From: ska Date: Fri, 7 Nov 2025 12:31:09 +0100 Subject: [PATCH 1/9] Refactor Odometry class - Rename SteeringOdometry to SteeringKinematics - Add new header and source file for SteeringKinematics class - Insert deprecated warning in steering odometry header file --- steering_controllers_library/CMakeLists.txt | 2 +- .../steering_kinematics.hpp | 308 ++++++++++++++++++ .../steering_odometry.hpp | 276 +--------------- ...g_odometry.cpp => steering_kinematics.cpp} | 50 +-- 4 files changed, 347 insertions(+), 289 deletions(-) create mode 100644 steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp rename steering_controllers_library/src/{steering_odometry.cpp => steering_kinematics.cpp} (88%) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index d57b00d278..0ea70f180a 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -39,7 +39,7 @@ add_library( steering_controllers_library SHARED src/steering_controllers_library.cpp - src/steering_odometry.cpp + src/steering_kinematics.cpp ) target_compile_features(steering_controllers_library PUBLIC cxx_std_17) target_include_directories(steering_controllers_library PUBLIC diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp new file mode 100644 index 0000000000..e9de8ab335 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp @@ -0,0 +1,308 @@ +// 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_KINEMATICS_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_KINEMATICS_HPP_ + +#include +#include +#include + +#include + +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 +#include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif + +namespace steering_kinematics +{ +const unsigned int BICYCLE_CONFIG = 0; +const unsigned int TRICYCLE_CONFIG = 1; +const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + +/** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ +class SteeringKinematics +{ +public: + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ + explicit SteeringKinematics(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest velocity command + * \param v_bx Linear velocity [m/s] + * \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); + + /** + * \brief Set odometry type + * \param type odometry type + */ + void set_odometry_type(const unsigned int type); + + /** + * \brief Get odometry type + * \return odometry type + */ + unsigned int get_odometry_type() const { return static_cast(config_type_); } + + /** + * \brief heading getter + * \return heading [rad] + */ + double get_heading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double get_x() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double get_y() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double get_linear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double get_angular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track + */ + void set_wheel_params( + const double wheel_radius, const double wheel_base = 0.0, const double wheel_track = 0.0); + + /** + * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track for steering and traction + */ + void set_wheel_params( + const double wheel_radius, const double wheel_base, const double wheel_track_steering, + const double wheel_track_traction); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); + + /** + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \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 reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * has been reached + * \return Tuple of velocity commands and steering commands + */ + std::tuple, std::vector> get_commands( + const double v_bx, const double omega_bz, const bool open_loop = true, + const bool reduce_wheel_speed_until_steering_reached = false); + + /** + * \brief Reset poses, heading, and accumulators + */ + void reset_odometry(); + +private: + /** + * \brief Uses precomputed linear and angular velocities to compute odometry + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + bool update_odometry(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Integrates the velocities (linear and angular) + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call + */ + void integrate_fk(const double v_bx, const double omega_bz, const double dt); + + /** + * \brief Calculates steering angle from the desired twist + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param omega_bz Angular velocity of the robot around x_z-axis + */ + double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); + + /** + * \brief Reset linear and angular accumulators + */ + void reset_accumulators(); + +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double steer_pos_; // [rad] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Kinematic parameters + double wheel_track_traction_; // [m] + double wheel_track_steering_; // [m] + double wheel_base_; // [m] + double wheel_radius_; // [m] + + /// Configuration type used for the forward kinematics + int config_type_ = -1; + + /// Previous wheel position/state [rad]: + double traction_wheel_old_pos_; + double traction_right_wheel_old_pos_; + double traction_left_wheel_old_pos_; + /// Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_acc_; + RollingMeanAccumulator angular_acc_; +}; +} // namespace steering_kinematics + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_KINEMATICS_HPP_ 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..26e35c8042 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,6 +18,10 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#warning \ + "This header (steering_odometry.hpp) is deprecated. Please include steering_kinematics.hpp instead." +#include "steering_controllers_library/steering_kinematics.hpp" + #include #include #include @@ -33,276 +37,22 @@ namespace steering_odometry { -const unsigned int BICYCLE_CONFIG = 0; -const unsigned int TRICYCLE_CONFIG = 1; -const unsigned int ACKERMANN_CONFIG = 2; +[[deprecated("Use steering_kinematics::BICYCLE_CONFIG")]] const unsigned int BICYCLE_CONFIG = + steering_kinematics::BICYCLE_CONFIG; +[[deprecated("Use steering_kinematics::TRICYCLE_CONFIG")]] const unsigned int TRICYCLE_CONFIG = + steering_kinematics::TRICYCLE_CONFIG; +[[deprecated("Use steering_kinematics::ACKERMANN_CONFIG")]] const unsigned int ACKERMANN_CONFIG = + steering_kinematics::ACKERMANN_CONFIG; -inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } +inline bool is_close_to_zero(double val) { return steering_kinematics::is_close_to_zero(val); } /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) */ -class SteeringOdometry -{ -public: - /** - * \brief Constructor - * Timestamp will get the current time value - * Value will be set to zero - * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean - * - */ - explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); - - /** - * \brief Initialize the odometry - * \param time Current time - */ - void init(const rclcpp::Time & time); - - /** - * \brief Updates the odometry class with latest wheels position - * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_from_position( - const double traction_wheel_pos, const double steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_pos Right traction wheel velocity [rad] - * \param left_traction_wheel_pos Left traction wheel velocity [rad] - * \param steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_from_position( - const double right_traction_wheel_pos, const double left_traction_wheel_pos, - const double steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_pos Right traction wheel position [rad] - * \param left_traction_wheel_pos Left traction wheel position [rad] - * \param right_steer_pos Right steer wheel position [rad] - * \param left_steer_pos Left steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_from_position( - const double right_traction_wheel_pos, const double left_traction_wheel_pos, - const double right_steer_pos, const double left_steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest wheels position - * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_from_velocity( - const double traction_wheel_vel, const double steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param steer_pos Steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_from_velocity( - const double right_traction_wheel_vel, const double left_traction_wheel_vel, - const double steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest wheels position - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param right_steer_pos Right steer wheel position [rad] - * \param left_steer_pos Left steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_from_velocity( - const double right_traction_wheel_vel, const double left_traction_wheel_vel, - const double right_steer_pos, const double left_steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest velocity command - * \param v_bx Linear velocity [m/s] - * \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); - - /** - * \brief Set odometry type - * \param type odometry type - */ - void set_odometry_type(const unsigned int type); - - /** - * \brief Get odometry type - * \return odometry type - */ - unsigned int get_odometry_type() const { return static_cast(config_type_); } - - /** - * \brief heading getter - * \return heading [rad] - */ - double get_heading() const { return heading_; } - - /** - * \brief x position getter - * \return x position [m] - */ - double get_x() const { return x_; } - - /** - * \brief y position getter - * \return y position [m] - */ - double get_y() const { return y_; } - - /** - * \brief linear velocity getter - * \return linear velocity [m/s] - */ - double get_linear() const { return linear_; } - - /** - * \brief angular velocity getter - * \return angular velocity [rad/s] - */ - double get_angular() const { return angular_; } - - /** - * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track - */ - void set_wheel_params( - const double wheel_radius, const double wheel_base = 0.0, const double wheel_track = 0.0); - - /** - * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track for steering and traction - */ - void set_wheel_params( - const double wheel_radius, const double wheel_base, const double wheel_track_steering, - const double wheel_track_traction); - - /** - * \brief Velocity rolling window size setter - * \param velocity_rolling_window_size Velocity rolling window size - */ - void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); - - /** - * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \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 reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle - * has been reached - * \return Tuple of velocity commands and steering commands - */ - std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz, const bool open_loop = true, - const bool reduce_wheel_speed_until_steering_reached = false); - - /** - * \brief Reset poses, heading, and accumulators - */ - void reset_odometry(); - -private: - /** - * \brief Uses precomputed linear and angular velocities to compute odometry - * \param v_bx Linear velocity [m/s] - * \param omega_bz Angular velocity [rad/s] - * \param dt time difference to last call - */ - bool update_odometry(const double v_bx, const double omega_bz, const double dt); - - /** - * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param v_bx Linear velocity [m/s] - * \param omega_bz Angular velocity [rad/s] - * \param dt time difference to last call - */ - void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); - - /** - * \brief Integrates the velocities (linear and angular) - * \param v_bx Linear velocity [m/s] - * \param omega_bz Angular velocity [rad/s] - * \param dt time difference to last call - */ - void integrate_fk(const double v_bx, const double omega_bz, const double dt); - - /** - * \brief Calculates steering angle from the desired twist - * \param v_bx Linear velocity of the robot in x_b-axis direction - * \param omega_bz Angular velocity of the robot around x_z-axis - */ - double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); - - /** - * \brief Calculates linear velocity of a robot with double traction axle - * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] - * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param steer_pos Steer wheel position [rad] - */ - double get_linear_velocity_double_traction_axle( - const double right_traction_wheel_vel, const double left_traction_wheel_vel, - const double steer_pos); - - /** - * \brief Reset linear and angular accumulators - */ - void reset_accumulators(); - -// \note The versions conditioning is added here to support the source-compatibility with Humble -#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 - using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; -#else - using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; -#endif - - /// Current timestamp: - rclcpp::Time timestamp_; - - /// Current pose: - double x_; // [m] - double y_; // [m] - double steer_pos_; // [rad] - double heading_; // [rad] - - /// Current velocity: - double linear_; // [m/s] - double angular_; // [rad/s] - - /// Kinematic parameters - double wheel_track_traction_; // [m] - double wheel_track_steering_; // [m] - double wheel_base_; // [m] - double wheel_radius_; // [m] - - /// Configuration type used for the forward kinematics - int config_type_ = -1; +using SteeringOdometry [[deprecated("Use steering_kinematics::SteeringKinematics instead.")]] = + steering_kinematics::SteeringKinematics; - /// Previous wheel position/state [rad]: - double traction_wheel_old_pos_; - double traction_right_wheel_old_pos_; - double traction_left_wheel_old_pos_; - /// Rolling mean accumulators for the linear and angular velocities: - size_t velocity_rolling_window_size_; - RollingMeanAccumulator linear_acc_; - RollingMeanAccumulator angular_acc_; -}; } // namespace steering_odometry #endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_kinematics.cpp similarity index 88% rename from steering_controllers_library/src/steering_odometry.cpp rename to steering_controllers_library/src/steering_kinematics.cpp index 0a5367f4bd..ef56bb00cd 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_kinematics.cpp @@ -19,14 +19,14 @@ #define _USE_MATH_DEFINES -#include "steering_controllers_library/steering_odometry.hpp" +#include "steering_controllers_library/steering_kinematics.hpp" #include #include -namespace steering_odometry +namespace steering_kinematics { -SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) +SteeringKinematics::SteeringKinematics(size_t velocity_rolling_window_size) : timestamp_(0.0), x_(0.0), y_(0.0), @@ -46,14 +46,14 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) { } -void SteeringOdometry::init(const rclcpp::Time & time) +void SteeringKinematics::init(const rclcpp::Time & time) { // Reset accumulators and timestamp: reset_accumulators(); timestamp_ = time; } -bool SteeringOdometry::update_odometry( +bool SteeringKinematics::update_odometry( const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: @@ -75,7 +75,7 @@ bool SteeringOdometry::update_odometry( return true; } -bool SteeringOdometry::update_from_position( +bool SteeringKinematics::update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt) { const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_; @@ -86,7 +86,7 @@ bool SteeringOdometry::update_from_position( return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt); } -bool SteeringOdometry::update_from_position( +bool SteeringKinematics::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double steer_pos, const double dt) { @@ -103,7 +103,7 @@ bool SteeringOdometry::update_from_position( traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt); } -bool SteeringOdometry::update_from_position( +bool SteeringKinematics::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double right_steer_pos, const double left_steer_pos, const double dt) { @@ -121,7 +121,7 @@ bool SteeringOdometry::update_from_position( left_steer_pos, dt); } -bool SteeringOdometry::update_from_velocity( +bool SteeringKinematics::update_from_velocity( const double traction_wheel_vel, const double steer_pos, const double dt) { steer_pos_ = steer_pos; @@ -131,7 +131,7 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } -double SteeringOdometry::get_linear_velocity_double_traction_axle( +double SteeringKinematics::get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos) { @@ -152,7 +152,7 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( return (vel_r + vel_l) * 0.5; } -bool SteeringOdometry::update_from_velocity( +bool SteeringKinematics::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt) { @@ -165,7 +165,7 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } -bool SteeringOdometry::update_from_velocity( +bool SteeringKinematics::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt) { @@ -185,7 +185,7 @@ bool SteeringOdometry::update_from_velocity( 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 SteeringKinematics::update_open_loop(const double v_bx, const double omega_bz, const double dt) { /// Save last linear and angular velocity: linear_ = v_bx; @@ -195,7 +195,7 @@ void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz integrate_fk(v_bx, omega_bz, dt); } -void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) +void SteeringKinematics::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) { wheel_radius_ = wheel_radius; wheel_base_ = wheel_base; @@ -203,7 +203,7 @@ void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, wheel_track_steering_ = wheel_track; } -void SteeringOdometry::set_wheel_params( +void SteeringKinematics::set_wheel_params( double wheel_radius, double wheel_base, double wheel_track_steering, double wheel_track_traction) { wheel_radius_ = wheel_radius; @@ -212,26 +212,26 @@ void SteeringOdometry::set_wheel_params( wheel_track_steering_ = wheel_track_steering; } -void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +void SteeringKinematics::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) { velocity_rolling_window_size_ = velocity_rolling_window_size; reset_accumulators(); } -void SteeringOdometry::set_odometry_type(const unsigned int type) +void SteeringKinematics::set_odometry_type(const unsigned int type) { config_type_ = static_cast(type); } -double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) +double SteeringKinematics::convert_twist_to_steering_angle(double v_bx, double omega_bz) { // phi can be nan if both v_bx and omega_bz are zero const auto phi = std::atan(omega_bz * wheel_base_ / v_bx); return std::isfinite(phi) ? phi : 0.0; } -std::tuple, std::vector> SteeringOdometry::get_commands( +std::tuple, std::vector> SteeringKinematics::get_commands( const double v_bx, const double omega_bz, const bool open_loop, const bool reduce_wheel_speed_until_steering_reached) { @@ -252,7 +252,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma } #endif // steering angle - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + phi = SteeringKinematics::convert_twist_to_steering_angle(v_bx, omega_bz); if (open_loop) { phi_IK = phi; @@ -346,7 +346,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma } } -void SteeringOdometry::reset_odometry() +void SteeringKinematics::reset_odometry() { x_ = 0.0; y_ = 0.0; @@ -354,7 +354,7 @@ void SteeringOdometry::reset_odometry() reset_accumulators(); } -void SteeringOdometry::integrate_runge_kutta_2( +void SteeringKinematics::integrate_runge_kutta_2( const double v_bx, const double omega_bz, const double dt) { // Compute intermediate value of the heading @@ -366,7 +366,7 @@ void SteeringOdometry::integrate_runge_kutta_2( heading_ += omega_bz * dt; } -void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) +void SteeringKinematics::integrate_fk(const double v_bx, const double omega_bz, const double dt) { const double delta_x_b = v_bx * dt; const double delta_theta = omega_bz * dt; @@ -387,10 +387,10 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co } } -void SteeringOdometry::reset_accumulators() +void SteeringKinematics::reset_accumulators() { linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); } -} // namespace steering_odometry +} // namespace steering_kinematics From 860267559dbd16f63138133cc0d92435a7387e1c Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sat, 8 Nov 2025 13:44:29 +0100 Subject: [PATCH 2/9] Update comments --- .../steering_kinematics.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp index e9de8ab335..5021f7342f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp @@ -40,8 +40,8 @@ const unsigned int ACKERMANN_CONFIG = 2; inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } /** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) + * \brief The SteeringKinematics class handles forward kinematics (odometry calculations) and + * inverse kinematics (getting commands) (2D pose and velocity with related timestamp) */ class SteeringKinematics { @@ -56,13 +56,13 @@ class SteeringKinematics explicit SteeringKinematics(size_t velocity_rolling_window_size = 10); /** - * \brief Initialize the odometry + * \brief Initialize the SteeringKinematics class * \param time Current time */ void init(const rclcpp::Time & time); /** - * \brief Updates the odometry class with latest wheels position + * \brief Updates the SteeringKinematics class with latest wheels position * \param traction_wheel_pos traction wheel position [rad] * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call @@ -72,7 +72,7 @@ class SteeringKinematics const double traction_wheel_pos, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position + * \brief Updates the SteeringKinematics class with latest wheels position * \param right_traction_wheel_pos Right traction wheel velocity [rad] * \param left_traction_wheel_pos Left traction wheel velocity [rad] * \param steer_pos Steer wheel position [rad] @@ -84,7 +84,7 @@ class SteeringKinematics const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position + * \brief Updates the SteeringKinematics class with latest wheels position * \param right_traction_wheel_pos Right traction wheel position [rad] * \param left_traction_wheel_pos Left traction wheel position [rad] * \param right_steer_pos Right steer wheel position [rad] @@ -97,7 +97,7 @@ class SteeringKinematics const double right_steer_pos, const double left_steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position + * \brief Updates the SteeringKinematics class with latest wheels position * \param traction_wheel_vel Traction wheel velocity [rad/s] * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call @@ -107,7 +107,7 @@ class SteeringKinematics const double traction_wheel_vel, const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position + * \brief Updates the SteeringKinematics class with latest wheels position * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] * \param steer_pos Steer wheel position [rad] @@ -119,7 +119,7 @@ class SteeringKinematics const double steer_pos, const double dt); /** - * \brief Updates the odometry class with latest wheels position + * \brief Updates the SteeringKinematics class with latest wheels position * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] * \param right_steer_pos Right steer wheel position [rad] @@ -132,7 +132,7 @@ class SteeringKinematics const double right_steer_pos, const double left_steer_pos, const double dt); /** - * \brief Updates the odometry class with latest velocity command + * \brief Updates the SteeringKinematics class with latest velocity command * \param v_bx Linear velocity [m/s] * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call From 0f53dcd2874115a5cfff935ff0c4a82d114e5de1 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sat, 8 Nov 2025 13:45:02 +0100 Subject: [PATCH 3/9] Run pre-commits --- steering_controllers_library/src/steering_kinematics.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_kinematics.cpp b/steering_controllers_library/src/steering_kinematics.cpp index ef56bb00cd..2d5e6640b6 100644 --- a/steering_controllers_library/src/steering_kinematics.cpp +++ b/steering_controllers_library/src/steering_kinematics.cpp @@ -195,7 +195,8 @@ void SteeringKinematics::update_open_loop(const double v_bx, const double omega_ integrate_fk(v_bx, omega_bz, dt); } -void SteeringKinematics::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) +void SteeringKinematics::set_wheel_params( + double wheel_radius, double wheel_base, double wheel_track) { wheel_radius_ = wheel_radius; wheel_base_ = wheel_base; From 59d1330fae7e7ec0444d2860abce726594f8babe Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sat, 8 Nov 2025 14:09:03 +0100 Subject: [PATCH 4/9] Update warning msg --- .../include/steering_controllers_library/steering_odometry.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 26e35c8042..2eff3d10e3 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,8 +18,7 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ -#warning \ - "This header (steering_odometry.hpp) is deprecated. Please include steering_kinematics.hpp instead." +#warning "This header is deprecated. Please include steering_kinematics.hpp." #include "steering_controllers_library/steering_kinematics.hpp" #include From 23f9b161678abb0a746acfc085fa085aafe92779 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 9 Nov 2025 09:45:45 +0100 Subject: [PATCH 5/9] Modify warning to be a portable as per OS --- .../steering_controllers_library/steering_odometry.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) 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 2eff3d10e3..2b5e95cd41 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,7 +18,14 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ -#warning "This header is deprecated. Please include steering_kinematics.hpp." +#ifdef _WIN32 +#pragma message( \ + "This header is deprecated. Please update your code to use 'steering_kinematics.hpp' header.") // NOLINT +#else +#warning \ + "This header is deprecated. Please update your code to use 'steering_kinematics.hpp' header." //NOLINT +#endif + #include "steering_controllers_library/steering_kinematics.hpp" #include From e705c635c187dc4863bea981f177a75804b78835 Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Sun, 9 Nov 2025 10:29:35 +0100 Subject: [PATCH 6/9] Update steering_odometry to steering_kinematics to deprecate warnings --- .../src/ackermann_steering_controller.cpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- .../steering_controllers_library.hpp | 4 +- .../src/steering_controllers_library.cpp | 24 +++--- .../test_steering_controllers_library.hpp | 2 +- .../test/test_steering_odometry.cpp | 80 +++++++++---------- .../src/tricycle_steering_controller.cpp | 2 +- 7 files changed, 58 insertions(+), 58 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 326f05a579..eccfeb3977 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -45,7 +45,7 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom odometry_.set_wheel_params( traction_wheels_radius, wheelbase, steering_track_width, traction_track_width); - odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odometry_.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index b10ec445b7..8d114c8aeb 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -35,7 +35,7 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet const double traction_wheel_radius = bicycle_params_.traction_wheel_radius; odometry_.set_wheel_params(traction_wheel_radius, wheelbase); - odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odometry_.set_odometry_type(steering_kinematics::BICYCLE_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); 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 6deabe8237..b5a670b480 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 @@ -33,7 +33,7 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "steering_controllers_library/steering_controllers_library_parameters.hpp" -#include "steering_controllers_library/steering_odometry.hpp" +#include "steering_controllers_library/steering_kinematics.hpp" namespace steering_controllers_library { @@ -106,7 +106,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl bool on_set_chained_mode(bool chained_mode) override; /// Odometry: - steering_odometry::SteeringOdometry odometry_; + steering_kinematics::SteeringKinematics odometry_; SteeringControllerStateMsg published_state_; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index dd57971f30..38e201623e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -85,7 +85,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( configure_odometry(); // Check if the number of traction joints is correct - if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + if (odometry_.get_odometry_type() == steering_kinematics::BICYCLE_CONFIG) { if (params_.traction_joints_names.size() != 1) { @@ -96,7 +96,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG) { if (params_.traction_joints_names.size() != 2) { @@ -107,7 +107,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::ACKERMANN_CONFIG) { if (params_.traction_joints_names.size() != 2) { @@ -119,7 +119,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( } } // Check if the number of steering joints is correct - if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + if (odometry_.get_odometry_type() == steering_kinematics::BICYCLE_CONFIG) { if (params_.steering_joints_names.size() != 1) { @@ -130,7 +130,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG) { if (params_.steering_joints_names.size() != 1) { @@ -141,7 +141,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::ACKERMANN_CONFIG) { if (params_.steering_joints_names.size() != 2) { @@ -158,7 +158,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( if (!params_.traction_joints_state_names.empty()) { - if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + if (odometry_.get_odometry_type() == steering_kinematics::BICYCLE_CONFIG) { if (params_.traction_joints_state_names.size() != 1) { @@ -170,7 +170,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG) { if (params_.traction_joints_state_names.size() != 2) { @@ -182,7 +182,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::ACKERMANN_CONFIG) { if (params_.traction_joints_state_names.size() != 2) { @@ -203,7 +203,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( if (!params_.steering_joints_state_names.empty()) { - if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + if (odometry_.get_odometry_type() == steering_kinematics::BICYCLE_CONFIG) { if (params_.steering_joints_state_names.size() != 1) { @@ -215,7 +215,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG) { if (params_.steering_joints_state_names.size() != 1) { @@ -227,7 +227,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } } - else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + else if (odometry_.get_odometry_type() == steering_kinematics::ACKERMANN_CONFIG) { if (params_.steering_joints_state_names.size() != 2) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 30b8e36bec..b386e5a85a 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -121,7 +121,7 @@ class TestableSteeringControllersLibrary { set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); odometry_.set_wheel_params(WHEELS_RADIUS_, WHEELBASE_, WHEELS_TRACK_); - odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odometry_.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index a211ac8bf4..0645f76f0f 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -16,15 +16,15 @@ #include -#include "steering_controllers_library/steering_odometry.hpp" +#include "steering_controllers_library/steering_kinematics.hpp" TEST(TestSteeringOdometry, initialize) { - EXPECT_NO_THROW(steering_odometry::SteeringOdometry()); + EXPECT_NO_THROW(steering_kinematics::SteeringKinematics()); - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 3.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); EXPECT_DOUBLE_EQ(odom.get_heading(), 0.); EXPECT_DOUBLE_EQ(odom.get_x(), 0.); EXPECT_DOUBLE_EQ(odom.get_y(), 0.); @@ -34,9 +34,9 @@ TEST(TestSteeringOdometry, initialize) TEST(TestSteeringOdometry, ackermann_odometry) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); @@ -46,9 +46,9 @@ TEST(TestSteeringOdometry, ackermann_odometry) TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); odom.update_open_loop(2., 0., 0.5); EXPECT_DOUBLE_EQ(odom.get_linear(), 2.); EXPECT_DOUBLE_EQ(odom.get_x(), 1.); @@ -57,9 +57,9 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); odom.update_open_loop(1., 1., 1.); EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); EXPECT_DOUBLE_EQ(odom.get_angular(), 1.); @@ -70,9 +70,9 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); odom.update_open_loop(1., -1., 1.); EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); @@ -82,9 +82,9 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) TEST(TestSteeringOdometry, ackermann_IK_linear) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); odom.update_open_loop(1., 0., 1.); auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel @@ -97,9 +97,9 @@ TEST(TestSteeringOdometry, ackermann_IK_linear) TEST(TestSteeringOdometry, ackermann_IK_left) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel @@ -112,9 +112,9 @@ TEST(TestSteeringOdometry, ackermann_IK_left) TEST(TestSteeringOdometry, ackermann_IK_right) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel @@ -127,9 +127,9 @@ TEST(TestSteeringOdometry, ackermann_IK_right) TEST(TestSteeringOdometry, ackermann_IK_right_steering_limited) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.set_odometry_type(steering_kinematics::ACKERMANN_CONFIG); { odom.update_from_position(0., -0.785, 1.); // already steered @@ -175,9 +175,9 @@ TEST(TestSteeringOdometry, ackermann_IK_right_steering_limited) TEST(TestSteeringOdometry, bicycle_IK_linear) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::BICYCLE_CONFIG); odom.update_open_loop(1., 0., 1.); auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel @@ -188,9 +188,9 @@ TEST(TestSteeringOdometry, bicycle_IK_linear) TEST(TestSteeringOdometry, bicycle_IK_left) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::BICYCLE_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel @@ -201,9 +201,9 @@ TEST(TestSteeringOdometry, bicycle_IK_left) TEST(TestSteeringOdometry, bicycle_IK_right) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::BICYCLE_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel @@ -214,9 +214,9 @@ TEST(TestSteeringOdometry, bicycle_IK_right) TEST(TestSteeringOdometry, bicycle_IK_right_steering_limited) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::BICYCLE_CONFIG); { odom.update_from_position(0., -0.785, 1.); // already steered @@ -270,9 +270,9 @@ TEST(TestSteeringOdometry, bicycle_IK_right_steering_limited) TEST(TestSteeringOdometry, bicycle_odometry) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::BICYCLE_CONFIG); ASSERT_TRUE(odom.update_from_velocity(1., .1, .1)); EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); @@ -284,9 +284,9 @@ TEST(TestSteeringOdometry, bicycle_odometry) TEST(TestSteeringOdometry, tricycle_IK_linear) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); odom.update_open_loop(1., 0., 1.); auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel @@ -298,9 +298,9 @@ TEST(TestSteeringOdometry, tricycle_IK_linear) TEST(TestSteeringOdometry, tricycle_IK_left) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel @@ -312,9 +312,9 @@ TEST(TestSteeringOdometry, tricycle_IK_left) TEST(TestSteeringOdometry, tricycle_IK_right) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel @@ -326,9 +326,9 @@ TEST(TestSteeringOdometry, tricycle_IK_right) TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); - odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); { odom.update_from_position(0., -0.785, 1.); // already steered @@ -369,9 +369,9 @@ TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited) TEST(TestSteeringOdometry, tricycle_odometry) { - steering_odometry::SteeringOdometry odom(1); + steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 43fdcd4421..d799ac4e5e 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -35,7 +35,7 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome const double wheelbase = tricycle_params_.wheelbase; odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_track_width); - odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odometry_.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); From 1a9b5158b47f801410da1b8512168cd9f124b84f Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Tue, 18 Nov 2025 14:38:19 +0100 Subject: [PATCH 7/9] Reorganize steering odometry to pass ABI checks for backward compatibility --- .../steering_odometry.hpp | 107 +++++++++++++++++- 1 file changed, 103 insertions(+), 4 deletions(-) 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 2b5e95cd41..783f2e9803 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -53,12 +53,111 @@ namespace steering_odometry inline bool is_close_to_zero(double val) { return steering_kinematics::is_close_to_zero(val); } /** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) + * \brief Deprecated Odometry class for backward ABI compatibility. + * Internally calling steering_kinematics::SteeringKinematics */ -using SteeringOdometry [[deprecated("Use steering_kinematics::SteeringKinematics instead.")]] = - steering_kinematics::SteeringKinematics; +class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] SteeringOdometry +{ +public: + explicit SteeringOdometry(size_t velocity_rolling_window_size = 10) + : sk_impl_(velocity_rolling_window_size) + { + } + + void init(const rclcpp::Time & time) { sk_impl_.init(time); } + + bool update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt) + { + sk_impl_.update_from_position(traction_wheel_pos, steer_pos, dt); + } + + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double steer_pos, const double dt) + { + sk_impl_.update_from_position(right_traction_wheel_pos, left_traction_wheel_pos, steer_pos, dt); + } + + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt) + { + sk_impl_.update_from_position( + right_traction_wheel_pos, left_traction_wheel_pos, right_steer_pos, left_steer_pos, dt); + } + + bool update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt) + { + sk_impl_.update_from_velocity(traction_wheel_vel, steer_pos, dt); + } + + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt) + { + sk_impl_.update_from_velocity(right_traction_wheel_vel, left_traction_wheel_vel, steer_pos, dt); + } + + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt) + { + sk_impl_.update_from_velocity( + right_traction_wheel_vel, left_traction_wheel_vel, right_steer_pos, left_steer_pos, dt); + } + + void update_open_loop(const double v_bx, const double omega_bz, const double dt) + { + sk_impl_.update_open_loop(v_bx, omega_bz, dt); + } + + void set_odometry_type(const unsigned int type) { sk_impl_.set_odometry_type(type); } + + unsigned int get_odometry_type() const { sk_impl_.get_odometry_type(); } + + double get_heading() const { sk_impl_.get_heading(); } + + double get_x() const { sk_impl_.get_x(); } + + double get_y() const { sk_impl_.get_y(); } + + double get_linear() const { sk_impl_.get_linear(); } + + double get_angular() const { sk_impl_.get_angular(); } + + void set_wheel_params( + const double wheel_radius, const double wheel_base = 0.0, const double wheel_track = 0.0) + { + sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track); + } + + void set_wheel_params( + const double wheel_radius, const double wheel_base, const double wheel_track_steering, + const double wheel_track_traction) + { + sk_impl_.set_wheel_params(wheel_radius, wheel_track_steering, wheel_track_traction); + } + + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size) + { + sk_impl_.set_velocity_rolling_window_size(velocity_rolling_window_size); + } + + std::tuple, std::vector> get_commands( + double v_bx, double omega_bz, bool open_loop = true, + bool reduce_wheel_speed_until_steering_reached = false) + { + return impl_.get_commands(v_bx, omega_bz, open_loop, reduce_wheel_speed_until_steering_reached); + } + + void reset_odometry() { impl_.reset_odometry(); } + +private: + steering_kinematics::SteeringKinematics sk_impl_; +}; } // namespace steering_odometry #endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ From ab4fce1588f6116639d48ca5573156c7621075ec Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Tue, 18 Nov 2025 15:48:51 +0100 Subject: [PATCH 8/9] Fix typo --- .../steering_controllers_library/steering_odometry.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 783f2e9803..be8ee02632 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -150,10 +150,11 @@ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] Stee double v_bx, double omega_bz, bool open_loop = true, bool reduce_wheel_speed_until_steering_reached = false) { - return impl_.get_commands(v_bx, omega_bz, open_loop, reduce_wheel_speed_until_steering_reached); + return sk_impl_.get_commands( + v_bx, omega_bz, open_loop, reduce_wheel_speed_until_steering_reached); } - void reset_odometry() { impl_.reset_odometry(); } + void reset_odometry() { sk_impl_.reset_odometry(); } private: steering_kinematics::SteeringKinematics sk_impl_; From 348e34e1bd4eb8f6b7e48719f98b5e63e3a803fc Mon Sep 17 00:00:00 2001 From: Sanjeev Kumar Date: Tue, 18 Nov 2025 17:03:16 +0100 Subject: [PATCH 9/9] Remove implementation redirection from steering odometry header and add to source file for ABI --- steering_controllers_library/CMakeLists.txt | 1 + .../steering_odometry.hpp | 82 ++++--------- .../src/steering_odometry.cpp | 113 ++++++++++++++++++ 3 files changed, 135 insertions(+), 61 deletions(-) create mode 100644 steering_controllers_library/src/steering_odometry.cpp diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 0ea70f180a..aa06d481a0 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -40,6 +40,7 @@ add_library( SHARED src/steering_controllers_library.cpp src/steering_kinematics.cpp + src/steering_odometry.cpp ) target_compile_features(steering_controllers_library PUBLIC cxx_std_17) target_include_directories(steering_controllers_library PUBLIC 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 be8ee02632..9fa95dd45e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -60,101 +60,61 @@ inline bool is_close_to_zero(double val) { return steering_kinematics::is_close_ class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] SteeringOdometry { public: - explicit SteeringOdometry(size_t velocity_rolling_window_size = 10) - : sk_impl_(velocity_rolling_window_size) - { - } + explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); - void init(const rclcpp::Time & time) { sk_impl_.init(time); } + void init(const rclcpp::Time & time); bool update_from_position( - const double traction_wheel_pos, const double steer_pos, const double dt) - { - sk_impl_.update_from_position(traction_wheel_pos, steer_pos, dt); - } + const double traction_wheel_pos, const double steer_pos, const double dt); bool update_from_position( const double right_traction_wheel_pos, const double left_traction_wheel_pos, - const double steer_pos, const double dt) - { - sk_impl_.update_from_position(right_traction_wheel_pos, left_traction_wheel_pos, steer_pos, dt); - } + const double steer_pos, const double dt); bool update_from_position( const double right_traction_wheel_pos, const double left_traction_wheel_pos, - const double right_steer_pos, const double left_steer_pos, const double dt) - { - sk_impl_.update_from_position( - right_traction_wheel_pos, left_traction_wheel_pos, right_steer_pos, left_steer_pos, dt); - } + const double right_steer_pos, const double left_steer_pos, const double dt); bool update_from_velocity( - const double traction_wheel_vel, const double steer_pos, const double dt) - { - sk_impl_.update_from_velocity(traction_wheel_vel, steer_pos, dt); - } + const double traction_wheel_vel, const double steer_pos, const double dt); bool update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, - const double steer_pos, const double dt) - { - sk_impl_.update_from_velocity(right_traction_wheel_vel, left_traction_wheel_vel, steer_pos, dt); - } + const double steer_pos, const double dt); bool update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, - const double right_steer_pos, const double left_steer_pos, const double dt) - { - sk_impl_.update_from_velocity( - right_traction_wheel_vel, left_traction_wheel_vel, right_steer_pos, left_steer_pos, dt); - } + const double right_steer_pos, const double left_steer_pos, const double dt); - void update_open_loop(const double v_bx, const double omega_bz, const double dt) - { - sk_impl_.update_open_loop(v_bx, omega_bz, dt); - } + void update_open_loop(const double v_bx, const double omega_bz, const double dt); - void set_odometry_type(const unsigned int type) { sk_impl_.set_odometry_type(type); } + void set_odometry_type(const unsigned int type); - unsigned int get_odometry_type() const { sk_impl_.get_odometry_type(); } + unsigned int get_odometry_type() const; + double get_heading() const; - double get_heading() const { sk_impl_.get_heading(); } + double get_x() const; - double get_x() const { sk_impl_.get_x(); } + double get_y() const; - double get_y() const { sk_impl_.get_y(); } + double get_linear() const; - double get_linear() const { sk_impl_.get_linear(); } - - double get_angular() const { sk_impl_.get_angular(); } + double get_angular() const; void set_wheel_params( - const double wheel_radius, const double wheel_base = 0.0, const double wheel_track = 0.0) - { - sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track); - } + const double wheel_radius, const double wheel_base, const double wheel_track); void set_wheel_params( const double wheel_radius, const double wheel_base, const double wheel_track_steering, - const double wheel_track_traction) - { - sk_impl_.set_wheel_params(wheel_radius, wheel_track_steering, wheel_track_traction); - } + const double wheel_track_traction); - void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size) - { - sk_impl_.set_velocity_rolling_window_size(velocity_rolling_window_size); - } + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); std::tuple, std::vector> get_commands( double v_bx, double omega_bz, bool open_loop = true, - bool reduce_wheel_speed_until_steering_reached = false) - { - return sk_impl_.get_commands( - v_bx, omega_bz, open_loop, reduce_wheel_speed_until_steering_reached); - } + bool reduce_wheel_speed_until_steering_reached = false); - void reset_odometry() { sk_impl_.reset_odometry(); } + void reset_odometry(); private: steering_kinematics::SteeringKinematics sk_impl_; diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp new file mode 100644 index 0000000000..f668230d5a --- /dev/null +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -0,0 +1,113 @@ +// 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. + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#define _USE_MATH_DEFINES + +#include "steering_controllers_library/steering_odometry.hpp" + +namespace steering_odometry +{ +SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) +: sk_impl_(velocity_rolling_window_size) +{ +} + +void SteeringOdometry::init(const rclcpp::Time & time) { sk_impl_.init(time); } + +bool SteeringOdometry::update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt) +{ + return sk_impl_.update_from_position(traction_wheel_pos, steer_pos, dt); +} + +bool SteeringOdometry::update_from_position( + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double steer_pos, const double dt) +{ + return sk_impl_.update_from_position( + traction_right_wheel_pos, traction_left_wheel_pos, steer_pos, dt); +} + +bool SteeringOdometry::update_from_position( + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt) +{ + return sk_impl_.update_from_position( + traction_right_wheel_pos, traction_left_wheel_pos, right_steer_pos, left_steer_pos, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt) +{ + return sk_impl_.update_from_velocity(traction_wheel_vel, steer_pos, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt) +{ + return sk_impl_.update_from_velocity( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt) +{ + return sk_impl_.update_from_velocity( + right_traction_wheel_vel, left_traction_wheel_vel, right_steer_pos, left_steer_pos, dt); +} + +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) +{ + sk_impl_.update_open_loop(v_bx, omega_bz, dt); +} + +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) +{ + sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track); +} + +void SteeringOdometry::set_wheel_params( + double wheel_radius, double wheel_base, double wheel_track_steering, double wheel_track_traction) +{ + sk_impl_.set_wheel_params(wheel_radius, wheel_base, wheel_track_steering, wheel_track_traction); +} + +void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +{ + sk_impl_.set_velocity_rolling_window_size(velocity_rolling_window_size); +} + +void SteeringOdometry::set_odometry_type(const unsigned int type) +{ + sk_impl_.set_odometry_type(type); +} + +std::tuple, std::vector> SteeringOdometry::get_commands( + const double v_bx, const double omega_bz, const bool open_loop, + const bool reduce_wheel_speed_until_steering_reached) +{ + return sk_impl_.get_commands( + v_bx, omega_bz, open_loop, reduce_wheel_speed_until_steering_reached); +} + +void SteeringOdometry::reset_odometry() { sk_impl_.reset_odometry(); } + +} // namespace steering_odometry