Hackjob for CPU consumption reduction.

rmf_fleet_adapter/launch/fleet_adapter.launch.xml

@@ -22,6 +22,7 @@
   <arg name="retry_wait" default="10.0" description="How long a retry should wait before starting"/>
   <arg name="discovery_timeout" default="10.0" description="How long to wait on discovery before giving up"/>
   <arg name="reversible" default="true" description="Can the robot drive backwards"/>
+  <arg name="transport_sleep" default="0.0" description="Use this value to limit the CPU usage."/>
   <arg name="output" default="screen"/>
 
   <node pkg="rmf_fleet_adapter"
@@ -51,6 +52,7 @@
     <param name="reversible" value="$(var reversible)"/>
 
     <param name="use_sim_time" value="$(var use_sim_time)"/>
+    <param name="transport_sleep" value="$(var transport_sleep)"/>
   </node>
 
 </launch>

rmf_fleet_adapter/rmf_rxcpp/include/rmf_rxcpp/Transport.hpp

@@ -23,6 +23,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rxcpp/rx.hpp>
 #include <utility>
+#include <optional>
 
 namespace rmf_rxcpp {
 
@@ -38,10 +39,12 @@ class Transport : public rclcpp::Node
   explicit Transport(
       rxcpp::schedulers::worker worker,
       const std::string& node_name,
-      const rclcpp::NodeOptions& options = rclcpp::NodeOptions())
+      const rclcpp::NodeOptions& options = rclcpp::NodeOptions(),
+      const std::optional<std::chrono::nanoseconds>& wait_time = std::nullopt)
     : rclcpp::Node{node_name, options},
       _worker{std::move(worker)},
-      _executor(_make_exec_args(options))
+      _executor(_make_exec_args(options)),
+      _wait_time(wait_time)
   {
     // Do nothing
   }
@@ -57,6 +60,27 @@ class Transport : public rclcpp::Node
     if (!_node_added)
       _executor.add_node(shared_from_this());
 
+    const auto sleep_param = "transport_sleep";
+    declare_parameter<double>(sleep_param, 0.0);
+    if (has_parameter(sleep_param))
+    {
+      auto param = get_parameter(sleep_param);
+      if (param.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE)
+      {
+        RCLCPP_WARN(get_logger(), "Expected parameter %s to be double", sleep_param);
+      }
+      else
+      {
+        auto sleep_time = param.as_double();
+        if(sleep_time > 0)
+        {
+          _wait_time = {std::chrono::duration_cast<std::chrono::nanoseconds>(
+            std::chrono::duration<double, std::ratio<1,1000>>(sleep_time))};
+        }
+        RCLCPP_WARN(get_logger(), "Sleeping for %f", sleep_time);
+      }
+    }
+
     _stopping = false;
     _schedule_spin();
   }
@@ -104,6 +128,7 @@ class Transport : public rclcpp::Node
   rclcpp::executors::SingleThreadedExecutor _executor;
   bool _node_added = false;
   std::condition_variable _spin_cv;
+  std::optional<std::chrono::nanoseconds> _wait_time;
 
   static rclcpp::ExecutorOptions _make_exec_args(
       const rclcpp::NodeOptions& options)
@@ -116,7 +141,10 @@ class Transport : public rclcpp::Node
   void _do_spin()
   {
     _executor.spin_some();
-
+    if (_wait_time)
+    {
+      rclcpp::sleep_for(*_wait_time);
+    }
     if (still_spinning())
       _schedule_spin();
   }