From 1b38a22db7fd5113fe529b04f5f494abc229981e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 1 Jan 2025 20:09:55 +0100 Subject: [PATCH] Fix open-loop odometry in case of ref timeout (#1454) Co-authored-by: Dr. Denis (cherry picked from commit 2375aca137a00b8d162babf5f8dbec2c35be55ba) --- .../src/steering_controllers_library.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 2d8c1baa66..223f174cd2 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -432,22 +432,22 @@ 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.0 : reference_interfaces_[0]; + last_angular_velocity_ = timeout ? 0.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) { 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++) { @@ -459,7 +459,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++) {