diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
index 002b4c9c4..923503cda 100644
--- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
+++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
@@ -22,6 +22,7 @@
+
+
diff --git a/rmf_fleet_adapter/rmf_rxcpp/include/rmf_rxcpp/Transport.hpp b/rmf_fleet_adapter/rmf_rxcpp/include/rmf_rxcpp/Transport.hpp
index 7b861748d..ffe6847d2 100644
--- a/rmf_fleet_adapter/rmf_rxcpp/include/rmf_rxcpp/Transport.hpp
+++ b/rmf_fleet_adapter/rmf_rxcpp/include/rmf_rxcpp/Transport.hpp
@@ -23,6 +23,7 @@
#include
#include
#include
+#include
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& 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(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::duration>(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 _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();
}