From ac291ab511545074564155f361673d8301550d74 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 --- .../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 3cfa056b27..95bcef7e63 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 af2736a8a3..eb497ebb3d 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -443,10 +443,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 0a7dc23ef2..bf254bfcfa 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -210,7 +210,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;