Add read_only_blockade adapter (#110)

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
open-rmf · Sep 1, 2021 · dd150e0 · dd150e0
1 parent 0652903
commit dd150e0
Show file tree

Hide file tree

Showing 16 changed files with 1,046 additions and 40 deletions.
diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt
@@ -166,6 +166,18 @@ target_link_libraries(read_only
 
 # -----------------------------------------------------------------------------
 
+add_executable(read_only_blockade
+  src/read_only_blockade/main.cpp
+  src/read_only_blockade/FleetAdapterNode.cpp
+)
+
+target_link_libraries(read_only_blockade
+  PRIVATE
+    rmf_fleet_adapter
+)
+
+# -----------------------------------------------------------------------------
+
 add_executable(full_control src/full_control/main.cpp)
 
 target_link_libraries(full_control
@@ -456,6 +468,7 @@ install(
   TARGETS
     rmf_fleet_adapter
     read_only
+    read_only_blockade
     mock_traffic_light
     full_control
     lift_supervisor

diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
@@ -4,7 +4,7 @@
 
   <arg name="use_sim_time" default="false" description="Use the /clock topic for time to sync with simulation"/>
 
-  <arg name="control_type" description="Fleet adapter type: full_control or read_only"/>
+  <arg name="control_type" description="Fleet adapter type: full_control, read_only, or read_only_blockade"/>
   <arg name="fleet_name" description="Name of the fleet that this adapter will interface with"/>
   <!-- Note: We give a blank default to nav_graph_file because it is not required for read_only
              mode. However it is still required for full_control mode.

diff --git a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
@@ -70,6 +70,15 @@ std::shared_ptr<FleetAdapterNode> FleetAdapterNode::make()
       node->_negotiation = rmf_traffic_ros2::schedule::Negotiation(
         *node, node->_mirror->snapshot_handle());
 
+      // Don't subscribe until everything else is ready
+      node->_fleet_state_subscription =
+        node->create_subscription<FleetState>(
+        FleetStateTopicName, rclcpp::SystemDefaultsQoS(),
+        [self = node.get()](FleetState::UniquePtr msg)
+        {
+          self->fleet_state_update(std::move(msg));
+        });
+
       return node;
     }
   }
@@ -173,13 +182,7 @@ FleetAdapterNode::FleetAdapterNode()
   _fleet_name(get_fleet_name_parameter(*this)),
   _traits(get_traits_or_default(*this, 0.7, 0.3, 0.5, 1.5, 0.5, 1.5))
 {
-  _fleet_state_subscription =
-    create_subscription<FleetState>(
-    FleetStateTopicName, rclcpp::SystemDefaultsQoS(),
-    [&](FleetState::UniquePtr msg)
-    {
-      this->fleet_state_update(std::move(msg));
-    });
+  // Do nothing. Everything else is initialized in make()
 }
 
 //==============================================================================

diff --git a/rmf_fleet_adapter/src/read_only/main.cpp b/rmf_fleet_adapter/src/read_only/main.cpp
@@ -17,12 +17,8 @@
 
 #include "FleetAdapterNode.hpp"
 
-#include <rmf_traffic/geometry/Circle.hpp>
-
 #include <rclcpp/executors.hpp>
 
-#include "../rmf_fleet_adapter/ParseArgs.hpp"
-
 using namespace rmf_fleet_adapter;
 
 int main(int argc, char* argv[])