From ead8808d6abfe8d3bf95cc87dfb53c5c19d5b702 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 3 Jan 2025 16:54:25 +0800
Subject: [PATCH 1/2] Rename capability and add rmf namespace

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 nexus_capabilities/CMakeLists.txt             |  4 +--
 .../src/capabilities/plugins.xml              |  4 +--
 .../{transport_amr.cpp => rmf_request.cpp}    | 13 ++++++----
 .../{transport_amr.hpp => rmf_request.hpp}    | 11 +++++---
 ...ability.cpp => rmf_request_capability.cpp} | 26 ++++++++++---------
 ...ability.hpp => rmf_request_capability.hpp} |  9 +++----
 .../config/rmf_bts/transportation.xml         | 16 ++++++------
 .../launch/control_center.launch.py           |  2 +-
 .../launch/office.launch.xml                  |  1 +
 9 files changed, 46 insertions(+), 40 deletions(-)
 rename nexus_capabilities/src/capabilities/{transport_amr.cpp => rmf_request.cpp} (97%)
 rename nexus_capabilities/src/capabilities/{transport_amr.hpp => rmf_request.hpp} (97%)
 rename nexus_capabilities/src/capabilities/{transport_amr_capability.cpp => rmf_request_capability.cpp} (72%)
 rename nexus_capabilities/src/capabilities/{transport_amr_capability.hpp => rmf_request_capability.hpp} (84%)

diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt
index 237e60e..6d0dda1 100644
--- a/nexus_capabilities/CMakeLists.txt
+++ b/nexus_capabilities/CMakeLists.txt
@@ -130,8 +130,8 @@ add_library(nexus_builtin_capabilities SHARED
   src/capabilities/gripper_control.cpp
   src/capabilities/plan_motion_capability.cpp
   src/capabilities/plan_motion.cpp
-  src/capabilities/transport_amr.cpp
-  src/capabilities/transport_amr_capability.cpp
+  src/capabilities/rmf_request.cpp
+  src/capabilities/rmf_request_capability.cpp
 )
 
 target_compile_options(nexus_builtin_capabilities PUBLIC INTERFACE cxx_std_17)
diff --git a/nexus_capabilities/src/capabilities/plugins.xml b/nexus_capabilities/src/capabilities/plugins.xml
index 61f6be7..85fe2c8 100644
--- a/nexus_capabilities/src/capabilities/plugins.xml
+++ b/nexus_capabilities/src/capabilities/plugins.xml
@@ -19,7 +19,7 @@
     <description>PlanMotion capability.</description>
   </class>
 
-  <class type="nexus::capabilities::TransportAmrCapability" base_class_type="nexus::Capability">
-    <description>TransportAmr capability.</description>
+  <class type="nexus::capabilities::RMFRequestCapability" base_class_type="nexus::Capability">
+    <description>RMFRequest capability.</description>
   </class>
 </library>
diff --git a/nexus_capabilities/src/capabilities/transport_amr.cpp b/nexus_capabilities/src/capabilities/rmf_request.cpp
similarity index 97%
rename from nexus_capabilities/src/capabilities/transport_amr.cpp
rename to nexus_capabilities/src/capabilities/rmf_request.cpp
index 296bd90..99f5a9d 100644
--- a/nexus_capabilities/src/capabilities/transport_amr.cpp
+++ b/nexus_capabilities/src/capabilities/rmf_request.cpp
@@ -15,14 +15,16 @@
  *
  */
 
-#include "transport_amr.hpp"
+#include "rmf_request.hpp"
 
 #include <nlohmann/json.hpp>
 #include <yaml-cpp/yaml.h>
 
 namespace nexus::capabilities {
 
-BT::NodeStatus DispatchRmfRequest::onStart()
+namespace rmf {
+
+BT::NodeStatus DispatchRequest::onStart()
 {
   const auto destinations = this->getInput<std::deque<AmrDestination>>("destinations");
   if (!destinations)
@@ -46,7 +48,7 @@ BT::NodeStatus DispatchRmfRequest::onStart()
   return BT::NodeStatus::RUNNING;
 }
 
-void DispatchRmfRequest::submit_itinerary(const std::deque<AmrDestination>& destinations)
+void DispatchRequest::submit_itinerary(const std::deque<AmrDestination>& destinations)
 {
   nlohmann::json j;
   j["type"] = "dispatch_task_request";
@@ -89,7 +91,7 @@ void DispatchRmfRequest::submit_itinerary(const std::deque<AmrDestination>& dest
   this->_api_request_pub->publish(msg);
 }
 
-void DispatchRmfRequest::api_response_cb(const ApiResponse& msg)
+void DispatchRequest::api_response_cb(const ApiResponse& msg)
 {
   // Receive response, populate hashmaps
   if (msg.type != msg.TYPE_RESPONDING)
@@ -119,7 +121,7 @@ void DispatchRmfRequest::api_response_cb(const ApiResponse& msg)
   this->rmf_task_id = j["state"]["booking"]["id"];
 }
 
-BT::NodeStatus DispatchRmfRequest::onRunning()
+BT::NodeStatus DispatchRequest::onRunning()
 {
   if (rmf_task_id.has_value())
   {
@@ -328,3 +330,4 @@ BT::NodeStatus SendSignal::tick()
 }
 
 }
+}
diff --git a/nexus_capabilities/src/capabilities/transport_amr.hpp b/nexus_capabilities/src/capabilities/rmf_request.hpp
similarity index 97%
rename from nexus_capabilities/src/capabilities/transport_amr.hpp
rename to nexus_capabilities/src/capabilities/rmf_request.hpp
index 23b00eb..5e8a596 100644
--- a/nexus_capabilities/src/capabilities/transport_amr.hpp
+++ b/nexus_capabilities/src/capabilities/rmf_request.hpp
@@ -15,8 +15,8 @@
  *
  */
 
-#ifndef NEXUS_WORKCELL_ORCHESTRATOR__TRANSPORT_AMR_HPP
-#define NEXUS_WORKCELL_ORCHESTRATOR__TRANSPORT_AMR_HPP
+#ifndef NEXUS_WORKCELL_ORCHESTRATOR__RMF_REQUEST_HPP
+#define NEXUS_WORKCELL_ORCHESTRATOR__RMF_REQUEST_HPP
 
 #include <nexus_capabilities/context_manager.hpp>
 #include <nexus_common/sync_service_client.hpp>
@@ -35,6 +35,8 @@
 
 namespace nexus::capabilities {
 
+namespace rmf {
+
 struct AmrDestination {
   // Either "pickup" or "dropoff"
   std::string action;
@@ -48,7 +50,7 @@ struct AmrDestination {
  *   signal |std::string| Signal to wait for.
  *   clear |bool| Set this to true to clear the signal when this node is finished.
  */
-class DispatchRmfRequest : public BT::StatefulActionNode
+class DispatchRequest : public BT::StatefulActionNode
 {
 // RMF interfaces
 public: using ApiRequest = rmf_task_msgs::msg::ApiRequest;
@@ -61,7 +63,7 @@ public: static BT::PortsList providedPorts()
     };
   }
 
-public: DispatchRmfRequest(const std::string& name,
+public: DispatchRequest(const std::string& name,
     const BT::NodeConfiguration& config,
     rclcpp_lifecycle::LifecycleNode::SharedPtr node)
   : BT::StatefulActionNode(name, config), _node(std::move(node)) {}
@@ -247,6 +249,7 @@ private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
 private: std::shared_ptr<const ContextManager> _ctx_mgr;
 };
 
+}
 }
 
 #endif
diff --git a/nexus_capabilities/src/capabilities/transport_amr_capability.cpp b/nexus_capabilities/src/capabilities/rmf_request_capability.cpp
similarity index 72%
rename from nexus_capabilities/src/capabilities/transport_amr_capability.cpp
rename to nexus_capabilities/src/capabilities/rmf_request_capability.cpp
index 649786d..f7ee054 100644
--- a/nexus_capabilities/src/capabilities/transport_amr_capability.cpp
+++ b/nexus_capabilities/src/capabilities/rmf_request_capability.cpp
@@ -17,59 +17,61 @@
 
 // #include <behaviortree_cpp/decorators/loop_node.h>
 
-#include "transport_amr.hpp"
-#include "transport_amr_capability.hpp"
+#include "rmf_request.hpp"
+#include "rmf_request_capability.hpp"
 
 namespace nexus::capabilities {
 
-void TransportAmrCapability::configure(
+using namespace rmf;
+
+void RMFRequestCapability::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   std::shared_ptr<const ContextManager> ctx_mgr,
   BT::BehaviorTreeFactory& bt_factory)
 {
-  bt_factory.registerBuilder<DispatchRmfRequest>("transport_amr.DispatchRmfRequest",
+  bt_factory.registerBuilder<DispatchRequest>("rmf_request.DispatchRequest",
     [this, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
-      return std::make_unique<DispatchRmfRequest>(name, config, node);
+      return std::make_unique<DispatchRequest>(name, config, node);
     });
 
-  bt_factory.registerBuilder<ExtractDestinations>("transport_amr.ExtractDestinations",
+  bt_factory.registerBuilder<ExtractDestinations>("rmf_request.ExtractDestinations",
     [this, node, ctx_mgr](const std::string& name,
     const BT::NodeConfiguration& config)
     {
       return std::make_unique<ExtractDestinations>(name, config, ctx_mgr, node);
     });
 
-  bt_factory.registerBuilder<UnpackDestinationData>("transport_amr.UnpackDestinationData",
+  bt_factory.registerBuilder<UnpackDestinationData>("rmf_request.UnpackDestinationData",
     [this, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
       return std::make_unique<UnpackDestinationData>(name, config, node);
     });
 
-  bt_factory.registerBuilder<SignalAmr>("transport_amr.SignalAmr",
+  bt_factory.registerBuilder<SignalAmr>("rmf_request.SignalAmr",
     [this, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
       return std::make_unique<SignalAmr>(name, config, node);
     });
 
-  bt_factory.registerBuilder<LoopDestinations>("transport_amr.LoopDestinations",
+  bt_factory.registerBuilder<LoopDestinations>("rmf_request.LoopDestinations",
     [this, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
       return std::make_unique<LoopDestinations>(name, config, node);
     });
 
-  bt_factory.registerBuilder<WaitForAmr>("transport_amr.WaitForAmr",
+  bt_factory.registerBuilder<WaitForAmr>("rmf_request.WaitForAmr",
     [this, node](const std::string& name,
     const BT::NodeConfiguration& config)
     {
       return std::make_unique<WaitForAmr>(name, config, node);
     });
 
-  bt_factory.registerBuilder<SendSignal>("transport_amr.SendSignal",
+  bt_factory.registerBuilder<SendSignal>("rmf_request.SendSignal",
     [this, node, ctx_mgr](const std::string& name,
     const BT::NodeConfiguration& config)
     {
@@ -82,6 +84,6 @@ void TransportAmrCapability::configure(
 #include <pluginlib/class_list_macros.hpp>
 
 PLUGINLIB_EXPORT_CLASS(
-  nexus::capabilities::TransportAmrCapability,
+  nexus::capabilities::RMFRequestCapability,
   nexus::Capability
 )
diff --git a/nexus_capabilities/src/capabilities/transport_amr_capability.hpp b/nexus_capabilities/src/capabilities/rmf_request_capability.hpp
similarity index 84%
rename from nexus_capabilities/src/capabilities/transport_amr_capability.hpp
rename to nexus_capabilities/src/capabilities/rmf_request_capability.hpp
index 1ce44a7..7a99e32 100644
--- a/nexus_capabilities/src/capabilities/transport_amr_capability.hpp
+++ b/nexus_capabilities/src/capabilities/rmf_request_capability.hpp
@@ -15,8 +15,8 @@
  *
  */
 
-#ifndef NEXUS_CAPABILITIES__CAPABILITIES__PLAN_MOTION_CAPABILITY_HPP
-#define NEXUS_CAPABILITIES__CAPABILITIES__PLAN_MOTION_CAPABILITY_HPP
+#ifndef NEXUS_CAPABILITIES__CAPABILITIES__RMF_REQUEST_CAPABILITY_HPP
+#define NEXUS_CAPABILITIES__CAPABILITIES__RMF_REQUEST_CAPABILITY_HPP
 
 #include <nexus_capabilities/capability.hpp>
 #include <nexus_capabilities/context_manager.hpp>
@@ -33,7 +33,7 @@ namespace nexus::capabilities {
 /**
  * Capability to plan robot arm motions.
  */
-class TransportAmrCapability : public Capability
+class RMFRequestCapability : public Capability
 {
   /**
    * @copydoc Capability::declare_params
@@ -58,9 +58,6 @@ public: void activate() final {}
    * @copydoc Capability::deactivate
    */
 public: void deactivate() final {}
-
-private: rclcpp::Client<endpoints::GetMotionPlanService::ServiceType>::
-  SharedPtr _client;
 };
 
 }
diff --git a/nexus_integration_tests/config/rmf_bts/transportation.xml b/nexus_integration_tests/config/rmf_bts/transportation.xml
index f74cc64..eedf981 100644
--- a/nexus_integration_tests/config/rmf_bts/transportation.xml
+++ b/nexus_integration_tests/config/rmf_bts/transportation.xml
@@ -3,23 +3,23 @@
   <BehaviorTree ID="PickAndPlace">
     <Sequence name="root_sequence">
       <!-- Extract the destination information from the task payload -->
-      <transport_amr.ExtractDestinations destinations="{destinations}" />
+      <rmf_request.ExtractDestinations destinations="{destinations}" />
       <!-- Sends the actual RMF task then returns -->
-      <transport_amr.DispatchRmfRequest destinations="{destinations}" rmf_task_id="{rmf_task_id}" />
-      <transport_amr.LoopDestinations queue="{destinations}" value="{destination}">
+      <rmf_request.DispatchRequest destinations="{destinations}" rmf_task_id="{rmf_task_id}" />
+      <rmf_request.LoopDestinations queue="{destinations}" value="{destination}">
         <Sequence>
           <!-- Utility node that unpacks the struct data into its fields -->
-          <transport_amr.UnpackDestinationData destination="{destination}" workcell="{workcell}" type="{type}" />
+          <rmf_request.UnpackDestinationData destination="{destination}" workcell="{workcell}" type="{type}" />
           <!-- Wait for AMR to reach the workcell -->
-          <transport_amr.WaitForAmr rmf_task_id="{rmf_task_id}" workcell="{workcell}" />
+          <rmf_request.WaitForAmr rmf_task_id="{rmf_task_id}" workcell="{workcell}" />
           <!-- Signal system orchestrator to start the workcell -->
-          <transport_amr.SendSignal signal="{workcell}" />
+          <rmf_request.SendSignal signal="{workcell}" />
           <!-- Wait for workcell to be done with its task -->
           <WaitForSignal signal="{workcell}" />
           <!-- Signal RMF that we can move -->
-          <transport_amr.SignalAmr workcell="{workcell}" rmf_task_id="{rmf_task_id}" />
+          <rmf_request.SignalAmr workcell="{workcell}" rmf_task_id="{rmf_task_id}" />
         </Sequence>
-      </transport_amr.LoopDestinations>
+      </rmf_request.LoopDestinations>
     </Sequence>
   </BehaviorTree>
 </root>
diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py
index 782b050..4810192 100644
--- a/nexus_integration_tests/launch/control_center.launch.py
+++ b/nexus_integration_tests/launch/control_center.launch.py
@@ -124,7 +124,7 @@ def launch_setup(context, *args, **kwargs):
         executable="nexus_workcell_orchestrator",
         parameters=[
             {
-                "capabilities": ["nexus::capabilities::TransportAmrCapability"],
+                "capabilities": ["nexus::capabilities::RMFRequestCapability"],
                 "bt_path": (FindPackageShare("nexus_integration_tests"), "/config/rmf_bts"),
             }
         ],
diff --git a/nexus_integration_tests/launch/office.launch.xml b/nexus_integration_tests/launch/office.launch.xml
index affb1ab..04d947b 100644
--- a/nexus_integration_tests/launch/office.launch.xml
+++ b/nexus_integration_tests/launch/office.launch.xml
@@ -31,6 +31,7 @@
   <!-- Simulator launch -->
   <include if="$(var use_simulator)" file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="headless" value="$(var headless)" />
+    <arg name="map_package" value="nexus_integration_tests" />
     <arg name="map_name" value="office" />
     <arg name="gazebo_version" value="8" />
     <arg name="sim_update_rate" value="$(var sim_update_rate)"/>

From 18070e67ee1cfb1c4e1b9dfcf377cf1a2b4b647d Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 3 Jan 2025 17:02:01 +0800
Subject: [PATCH 2/2] AmrDestination -> Destination

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 .../src/capabilities/rmf_request.cpp             | 12 ++++++------
 .../src/capabilities/rmf_request.hpp             | 16 ++++++++--------
 2 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/nexus_capabilities/src/capabilities/rmf_request.cpp b/nexus_capabilities/src/capabilities/rmf_request.cpp
index 99f5a9d..2bb63f6 100644
--- a/nexus_capabilities/src/capabilities/rmf_request.cpp
+++ b/nexus_capabilities/src/capabilities/rmf_request.cpp
@@ -26,7 +26,7 @@ namespace rmf {
 
 BT::NodeStatus DispatchRequest::onStart()
 {
-  const auto destinations = this->getInput<std::deque<AmrDestination>>("destinations");
+  const auto destinations = this->getInput<std::deque<Destination>>("destinations");
   if (!destinations)
   {
     RCLCPP_ERROR(
@@ -48,7 +48,7 @@ BT::NodeStatus DispatchRequest::onStart()
   return BT::NodeStatus::RUNNING;
 }
 
-void DispatchRequest::submit_itinerary(const std::deque<AmrDestination>& destinations)
+void DispatchRequest::submit_itinerary(const std::deque<Destination>& destinations)
 {
   nlohmann::json j;
   j["type"] = "dispatch_task_request";
@@ -135,14 +135,14 @@ BT::NodeStatus ExtractDestinations::tick()
 {
   const auto ctx = this->_ctx_mgr->current_context();
   const auto& task = ctx->task;
-  std::deque<AmrDestination> destinations;
+  std::deque<Destination> destinations;
   for (const auto& node : task.data)
   {
     if (node["type"] && node["destination"])
     {
       auto type = node["type"].as<std::string>();
       auto destination = node["destination"].as<std::string>();
-      destinations.push_back(AmrDestination {type, destination});
+      destinations.push_back(Destination {type, destination});
     }
     else
     {
@@ -158,7 +158,7 @@ BT::NodeStatus ExtractDestinations::tick()
 
 BT::NodeStatus UnpackDestinationData::tick()
 {
-  const auto destination = this->getInput<AmrDestination>("destination");
+  const auto destination = this->getInput<Destination>("destination");
   if (!destination)
   {
     RCLCPP_ERROR(
@@ -203,7 +203,7 @@ BT::NodeStatus LoopDestinations::tick()
 {
   if (!this->_initialized)
   {
-    auto queue = this->getInput<std::deque<AmrDestination>>("queue");
+    auto queue = this->getInput<std::deque<Destination>>("queue");
     if (!queue)
     {
       RCLCPP_ERROR(
diff --git a/nexus_capabilities/src/capabilities/rmf_request.hpp b/nexus_capabilities/src/capabilities/rmf_request.hpp
index 5e8a596..56df2ad 100644
--- a/nexus_capabilities/src/capabilities/rmf_request.hpp
+++ b/nexus_capabilities/src/capabilities/rmf_request.hpp
@@ -37,7 +37,7 @@ namespace nexus::capabilities {
 
 namespace rmf {
 
-struct AmrDestination {
+struct Destination {
   // Either "pickup" or "dropoff"
   std::string action;
   std::string workcell;
@@ -58,7 +58,7 @@ public: using ApiResponse = rmf_task_msgs::msg::ApiResponse;
 
 public: static BT::PortsList providedPorts()
   {
-    return { BT::InputPort<std::deque<AmrDestination>>("destinations", "Destinations to visit"),
+    return { BT::InputPort<std::deque<Destination>>("destinations", "Destinations to visit"),
       BT::OutputPort<std::string>("rmf_task_id", "The resulting RMF task id")
     };
   }
@@ -74,7 +74,7 @@ public: BT::NodeStatus onRunning() override;
 
 public: void onHalted() override {}
 
-private: void submit_itinerary(const std::deque<AmrDestination>& destinations);
+private: void submit_itinerary(const std::deque<Destination>& destinations);
 
 private: void api_response_cb(const ApiResponse& msg);
 
@@ -94,7 +94,7 @@ class ExtractDestinations : public BT::SyncActionNode
 public: static BT::PortsList providedPorts()
   {
     return {
-      BT::OutputPort<std::deque<AmrDestination>>("destinations"),
+      BT::OutputPort<std::deque<Destination>>("destinations"),
     };
   }
 
@@ -115,7 +115,7 @@ class UnpackDestinationData : public BT::SyncActionNode
 public: static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<AmrDestination>("destination"),
+      BT::InputPort<Destination>("destination"),
       BT::OutputPort<std::string>("workcell"),
       BT::OutputPort<std::string>("type"),
     };
@@ -165,8 +165,8 @@ class LoopDestinations : public BT::DecoratorNode
 public: static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::deque<AmrDestination>>("queue"),
-      BT::OutputPort<AmrDestination>("value"),
+      BT::InputPort<std::deque<Destination>>("queue"),
+      BT::OutputPort<Destination>("value"),
     };
   }
 
@@ -183,7 +183,7 @@ public: BT::NodeStatus tick() override;
 
 private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
 private: bool _initialized = false;
-private: std::deque<AmrDestination> _queue;
+private: std::deque<Destination> _queue;
 };
 
 class WaitForAmr: public BT::StatefulActionNode