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(); }