From 3f28f8f5e73c2430db1b2697c86b34aff8f91237 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 9 Oct 2023 16:24:45 +0000 Subject: [PATCH 1/3] set last*velocity variables for open loop odometry --- .../src/steering_controllers_library.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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++) From 2fc69351c94c291489d3174903098ea3f1bbb489 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 9 Oct 2023 16:35:22 +0000 Subject: [PATCH 2/3] Make function arguments const --- steering_controllers_library/src/steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; From 693c4f5fb98827b15d102e908dc187e9f27f4481 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 9 Oct 2023 16:52:18 +0000 Subject: [PATCH 3/3] Update function in header file too --- .../include/steering_controllers_library/steering_odometry.hpp | 3 ++- 1 file changed, 2 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 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