Implement push retry

ros-controls · Jan 9, 2025 · ce50a73 · ce50a73
1 parent fd6c708
commit ce50a73
Showing 1 changed file with 11 additions and 1 deletion.
diff --git 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