Merge branch 'luca/rmf_transporter' into ac/demo-map

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

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)

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>

nexus_capabilities/src/capabilities/transport_amr.cpp → nexus_capabilities/src/capabilities/rmf_request.cpp

@@ -15,16 +15,18 @@
  *
  */
 
-#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");
+  const auto destinations = this->getInput<std::deque<Destination>>("destinations");
   if (!destinations)
   {
     RCLCPP_ERROR(
@@ -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<Destination>& 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())
   {
@@ -133,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
     {
@@ -156,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(
@@ -201,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(
@@ -328,3 +330,4 @@ BT::NodeStatus SendSignal::tick()
 }
 
 }
+}

nexus_capabilities/src/capabilities/transport_amr.hpp → 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,7 +35,9 @@
 
 namespace nexus::capabilities {
 
-struct AmrDestination {
+namespace rmf {
+
+struct Destination {
   // Either "pickup" or "dropoff"
   std::string action;
   std::string workcell;
@@ -48,20 +50,20 @@ 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;
 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")
     };
   }
 
-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)) {}
@@ -72,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);
 
@@ -92,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"),
     };
   }
 
@@ -113,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"),
     };
@@ -163,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"),
     };
   }
 
@@ -181,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
@@ -247,6 +249,7 @@ private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
 private: std::shared_ptr<const ContextManager> _ctx_mgr;
 };
 
+}
 }
 
 #endif

nexus_capabilities/src/capabilities/transport_amr_capability.cpp → 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
 )

nexus_capabilities/src/capabilities/transport_amr_capability.hpp → 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;
 };
 
 }

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>

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"),
             }
         ],