From ce50a732bb7ce677998b532e0b32db1810e1ad1a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 9 Jan 2025 10:13:36 +0000 Subject: [PATCH] Implement push retry --- diff_drive_controller/src/diff_drive_controller.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 238c9549af..a424d946e2 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -420,7 +420,17 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - (void)received_velocity_msg_ptr_.bounded_push(std::move(msg)); + for (size_t i = 0; i < 5; ++i) + { + if (received_velocity_msg_ptr_.bounded_push(msg)) + { + break; + } + RCLCPP_WARN( + get_node()->get_logger(), + "Velocity command could not be stored in the queue, trying again"); + std::this_thread::sleep_for(100us); + } }); // initialize odometry publisher and message