From 1a1686dd7ab3896adc2d33261591aa90664f7708 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Oct 2023 13:00:53 +0200 Subject: [PATCH] Steering controllers library: fix open loop mode (#793) * set last*velocity variables for open loop odometry * Make function arguments const * Update function in header file too (cherry picked from commit ac291ab511545074564155f361673d8301550d74) --- .../steering_controllers_library/steering_odometry.hpp | 3 ++- .../src/steering_controllers_library.cpp | 7 ++++--- steering_controllers_library/src/steering_odometry.cpp | 2 +- 3 files changed, 7 insertions(+), 5 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 00516b716f..25d0235818 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -184,7 +184,8 @@ class SteeringOdometry * \param theta_dot Desired angular velocity [rad/s] * \return Tuple of velocity commands and steering commands */ - std::tuple, std::vector> get_commands(double Vx, double theta_dot); + std::tuple, std::vector> get_commands( + const double Vx, const double theta_dot); /** * \brief Reset poses, heading, and accumulators diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 179fc39869..26207d54b7 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -448,10 +448,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { // store and set commands - const double linear_command = reference_interfaces_[0]; - const double angular_command = reference_interfaces_[1]; + last_linear_velocity_ = reference_interfaces_[0]; + last_angular_velocity_ = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = - odometry_.get_commands(linear_command, angular_command); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 98a4f656f4..e8c3965b3f 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -212,7 +212,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub } std::tuple, std::vector> SteeringOdometry::get_commands( - double Vx, double theta_dot) + const double Vx, const double theta_dot) { // desired velocity and steering angle of the middle of traction and steering axis double Ws, alpha;