From b24515538cd31b6c2b64268465dd479f464b151d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 21:50:09 +0200 Subject: [PATCH] Fix steering controllers library code documentation and naming (#1149) * Update documentation and consolidate variable names * Simplify private methods and further update docs * Rename methods * Rename method and variables * Rename convert method * Rename variables and improve doc * Rename local variables * Use std::isfinite instead of !isnan Co-authored-by: Sai Kishor Kothakota * Use a lowercase theta for heading Co-authored-by: Sai Kishor Kothakota * Make some temporary variables const * Let update_from_position call update_from_velocity * Explicitly set variables with 0 in constructor * Fix docstring * Apply consistent variable naming Co-authored-by: Quique Llorente --------- Co-authored-by: Sai Kishor Kothakota Co-authored-by: Quique Llorente --- .../src/ackermann_steering_controller.cpp | 23 +-- .../src/bicycle_steering_controller.cpp | 10 +- .../steering_odometry.hpp | 63 +++---- .../src/steering_odometry.cpp | 175 ++++++++---------- .../src/tricycle_steering_controller.cpp | 18 +- 5 files changed, 139 insertions(+), 150 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c3a7539c5a..d9d95bf8b5 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double front_right_steer_position = - state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } } } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 5f013d7d7a..95eaf1965c 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds()); } } } 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 e4a22f6d3b..0104b011c7 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -61,7 +61,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Front Steer position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -72,7 +72,7 @@ class SteeringOdometry * \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 front_steer_pos Steer 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 */ @@ -96,7 +96,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param front_steer_pos Steer 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 */ @@ -107,7 +107,7 @@ class SteeringOdometry * \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 front_steer_pos Steer 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 */ @@ -130,11 +130,11 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time + * \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 linear, const double angular, const double dt); + void update_open_loop(const double v_bx, const double omega_bz, const double dt); /** * \brief Set odometry type @@ -175,22 +175,23 @@ class SteeringOdometry /** * \brief Sets the wheel parameters: radius, separation and wheelbase */ - void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + void set_wheel_params( + const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); /** * \brief Velocity rolling window size setter * \param velocity_rolling_window_size Velocity rolling window size */ - void set_velocity_rolling_window_size(size_t 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 Vx Desired linear velocity [m/s] - * \param theta_dot Desired angular velocity [rad/s] + * \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 * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double Vx, const double theta_dot); + const double v_bx, const double omega_bz); /** * \brief Reset poses, heading, and accumulators @@ -199,35 +200,35 @@ class SteeringOdometry private: /** - * \brief Uses precomputed linear and angular velocities to compute dometry and update - * accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) - * computed by previous odometry method \param angular Angular velocity [rad] (angular - * displacement, i.e. m/s * dt) computed by previous odometry method + * \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 linear_velocity, const double angular, const double dt); + 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 linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \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(double linear, double angular); + void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \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_exact(double linear, double angular); + void integrate_fk(const double v_bx, const double omega_bz, const double dt); /** - * \brief Calculates steering angle from the desired translational and rotational velocity - * \param Vx Linear velocity [m] - * \param theta_dot Angular velocity [rad] + * \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_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); /** * \brief Reset linear and angular accumulators diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 6fb20478a4..32cdb69454 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -35,6 +35,8 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) wheelbase_(0.0), wheel_radius_(0.0), traction_wheel_old_pos_(0.0), + traction_right_wheel_old_pos_(0.0), + traction_left_wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_acc_(velocity_rolling_window_size), angular_acc_(velocity_rolling_window_size) @@ -49,10 +51,10 @@ void SteeringOdometry::init(const rclcpp::Time & time) } bool SteeringOdometry::update_odometry( - const double linear_velocity, const double angular, const double dt) + const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); + integrate_fk(linear_velocity, angular_velocity, dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +64,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular); + angular_acc_.accumulate(angular_velocity); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean(); @@ -73,70 +75,47 @@ bool SteeringOdometry::update_odometry( bool SteeringOdometry::update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_; - const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_; + const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_; /// Update old position with current: - traction_wheel_old_pos_ = traction_wheel_cur_pos; + traction_wheel_old_pos_ = traction_wheel_pos; - /// Compute linear and angular diff: - const double linear_velocity = traction_wheel_est_pos_diff / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity(traction_wheel_est_pos_diff / dt, 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) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; - - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, 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) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; - - /// Compute linear and angular diff: - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos, + left_steer_pos, dt); } bool SteeringOdometry::update_from_velocity( @@ -144,9 +123,9 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( @@ -157,9 +136,9 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( @@ -169,19 +148,19 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; double linear_velocity = (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) { /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; + linear_ = v_bx; + angular_ = omega_bz; /// Integrate odometry: - SteeringOdometry::integrate_exact(linear * dt, angular * dt); + integrate_fk(v_bx, omega_bz, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) @@ -200,36 +179,37 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } -double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) +double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (theta_dot == 0 || Vx == 0) + if (omega_bz == 0 || v_bx == 0) { return 0; } - return std::atan(theta_dot * wheelbase_ / Vx); + return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double Vx, const double theta_dot) + const double v_bx, const double omega_bz) { - // desired velocity and steering angle of the middle of traction and steering axis - double Ws, alpha; + // desired wheel speed and steering angle of the middle of traction and steering axis + double Ws, phi; - if (Vx == 0 && theta_dot != 0) + if (v_bx == 0 && omega_bz != 0) { - alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); - Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); } if (config_type_ == BICYCLE_CONFIG) { std::vector traction_commands = {Ws}; - std::vector steering_commands = {alpha}; + std::vector steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == TRICYCLE_CONFIG) @@ -242,12 +222,12 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } - steering_commands = {alpha}; + steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == ACKERMANN_CONFIG) @@ -257,21 +237,23 @@ std::tuple, std::vector> SteeringOdometry::get_comma if (fabs(steer_pos_) < 1e-6) { traction_commands = {Ws, Ws}; - steering_commands = {alpha, alpha}; + steering_commands = {phi, phi}; } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; - double numerator = 2 * wheelbase_ * std::sin(alpha); - double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); - double denominator_second_member = wheel_track_ * std::sin(alpha); + const double numerator = 2 * wheelbase_ * std::sin(phi); + const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); + const double denominator_second_member = wheel_track_ * std::sin(phi); - double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); + const double alpha_r = + std::atan2(numerator, denominator_first_member + denominator_second_member); + const double alpha_l = + std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); @@ -290,35 +272,36 @@ void SteeringOdometry::reset_odometry() reset_accumulators(); } -void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) +void SteeringOdometry::integrate_runge_kutta_2( + const double v_bx, const double omega_bz, const double dt) { - const double direction = heading_ + angular * 0.5; + // Compute intermediate value of the heading + const double theta_mid = heading_ + omega_bz * 0.5 * dt; - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; + // Use the intermediate values to update the state + x_ += v_bx * cos(theta_mid) * dt; + y_ += v_bx * sin(theta_mid) * dt; + heading_ += omega_bz * dt; } -/** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ -void SteeringOdometry::integrate_exact(double linear, double angular) +void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) { - if (fabs(angular) < 1e-6) + const double delta_x_b = v_bx * dt; + const double delta_theta = omega_bz * dt; + + if (fabs(delta_theta) < 1e-6) { - integrate_runge_kutta_2(linear, angular); + /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): + integrate_runge_kutta_2(v_bx, omega_bz, dt); } else { - /// Exact integration (should solve problems when angular is zero): + /// Exact integration const double heading_old = heading_; - const double r = linear / angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); + const double R = delta_x_b / delta_theta; + heading_ += delta_theta; + x_ += R * (sin(heading_) - sin(heading_old)); + y_ += -R * (cos(heading_) - cos(heading_old)); } } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index f89d78a52c..03be6b88f0 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -60,24 +60,28 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } } }