From ff8bb575b0779ad546593fd8c8564c273b55abdd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 29 Dec 2024 20:21:06 +0000 Subject: [PATCH 1/3] Fix OL odometry in case of ref timeout --- .../src/steering_controllers_library.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3d4164080c..a0b0d2f668 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -378,16 +378,16 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store (for open loop odometry) and set commands - last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; - const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + // store (for open loop odometry) and set commands + last_linear_velocity_ = timeout ? 0. : reference_interfaces_[0]; + last_angular_velocity_ = timeout ? 0. : reference_interfaces_[1]; + auto [traction_commands, steering_commands] = odometry_.get_commands( - last_linear_velocity_, last_angular_velocity_, params_.open_loop, + reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); if (params_.front_steering) From a16b7a70613c322416de29b9e1fc90216720f403 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 1 Jan 2025 19:30:33 +0100 Subject: [PATCH 2/3] Update steering_controllers_library/src/steering_controllers_library.cpp --- .../src/steering_controllers_library.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a0b0d2f668..c71c634266 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -383,8 +383,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); // store (for open loop odometry) and set commands - last_linear_velocity_ = timeout ? 0. : reference_interfaces_[0]; - last_angular_velocity_ = timeout ? 0. : reference_interfaces_[1]; + last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; + last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, From 1e899c755521ea334d7cd417cfac61d6e7faa4f6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 1 Jan 2025 18:36:49 +0000 Subject: [PATCH 3/3] Reformat 0.0 --- .../src/steering_controllers_library.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index c71c634266..2d47746fed 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -394,7 +394,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -406,7 +406,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) {