diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt
index 285983d..99ebe25 100644
--- a/nexus_capabilities/CMakeLists.txt
+++ b/nexus_capabilities/CMakeLists.txt
@@ -15,8 +15,11 @@ set(dep_pkgs
nexus_common
nexus_endpoints
nexus_orchestrator_msgs
+ nlohmann_json
rclcpp
rclcpp_lifecycle
+ rmf_dispenser_msgs
+ rmf_task_msgs
yaml-cpp
)
foreach(pkg ${dep_pkgs})
@@ -125,6 +128,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
)
target_compile_options(nexus_builtin_capabilities PUBLIC INTERFACE cxx_std_17)
@@ -134,9 +139,13 @@ target_link_libraries(nexus_builtin_capabilities
${PROJECT_NAME}
nexus_common::nexus_common
nexus_endpoints::nexus_endpoints
+ nlohmann_json::nlohmann_json
pluginlib::pluginlib
tf2::tf2
tf2_ros::tf2_ros
+ ${rmf_dispenser_msgs_TARGETS}
+ ${rmf_task_msgs_TARGETS}
+ yaml-cpp
)
install(
diff --git a/nexus_capabilities/src/capabilities/plugins.xml b/nexus_capabilities/src/capabilities/plugins.xml
index c2b87bf..61f6be7 100644
--- a/nexus_capabilities/src/capabilities/plugins.xml
+++ b/nexus_capabilities/src/capabilities/plugins.xml
@@ -18,4 +18,8 @@
PlanMotion capability.
+
+
+ TransportAmr capability.
+
diff --git a/nexus_capabilities/src/capabilities/transport_amr.cpp b/nexus_capabilities/src/capabilities/transport_amr.cpp
new file mode 100644
index 0000000..adc9d91
--- /dev/null
+++ b/nexus_capabilities/src/capabilities/transport_amr.cpp
@@ -0,0 +1,336 @@
+/*
+ * Copyright (C) 2023 Johnson & Johnson
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "transport_amr.hpp"
+
+#include
+#include
+
+namespace nexus::capabilities {
+
+BT::NodeStatus DispatchRmfRequest::onStart()
+{
+ const auto destinations = this->getInput>("destinations");
+ if (!destinations)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [destinations] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ const auto transient_qos =
+ rclcpp::SystemDefaultsQoS().transient_local().keep_last(10).reliable();
+ this->_api_request_pub = this->_node->create_publisher("/task_api_requests", transient_qos);
+ // TODO(luca) publish request here with unique UUID
+ this->_api_response_sub = this->_node->create_subscription("/task_api_responses", transient_qos,
+ [&](ApiResponse::UniquePtr msg)
+ {
+ this->api_response_cb(*msg);
+ });
+ this->submit_itinerary(*destinations);
+ return BT::NodeStatus::RUNNING;
+}
+
+void DispatchRmfRequest::submit_itinerary(const std::deque& destinations)
+{
+ nlohmann::json j;
+ j["type"] = "dispatch_task_request";
+ nlohmann::json r;
+ r["unix_millis_request_time"] = 0;
+ r["unix_millis_earliest_start_time"] = 0;
+ r["requester"] = this->_node->get_name();
+ r["category"] = "compose";
+ nlohmann::json d;
+ d["category"] = "multi_delivery";
+ d["phases"] = nlohmann::json::array();
+ nlohmann::json activity;
+ activity["category"] = "sequence";
+ activity["description"]["activities"] = nlohmann::json::array();
+ for (const auto& destination : destinations)
+ {
+ nlohmann::json a;
+ a["category"] = destination.action;
+ nlohmann::json p;
+ // TODO(luca) exception safety for wrong types? Checking for pickup only since we don't do ingestors yet?
+ p["place"] = destination.workcell;
+ // TODO(luca) We should assign a handler that is related to the workcell.
+ // For now the assumption is that a location has only one handler
+ p["handler"] = destination.workcell;
+ p["payload"] = nlohmann::json::array();
+ a["description"] = p;
+ activity["description"]["activities"].push_back(a);
+ }
+ nlohmann::json act_obj;
+ act_obj["activity"] = activity;
+ d["phases"].push_back(act_obj);
+ r["description"] = d;
+ j["request"] = r;
+ ApiRequest msg;
+ msg.json_msg = j.dump();
+ // Time since epoch as a unique id
+ auto now = std::chrono::steady_clock::now().time_since_epoch();
+ msg.request_id = std::chrono::duration_cast(now).count();
+ this->requested_id = msg.request_id;
+ this->_api_request_pub->publish(msg);
+}
+
+void DispatchRmfRequest::api_response_cb(const ApiResponse& msg)
+{
+ // Receive response, populate hashmaps
+ if (msg.type != msg.TYPE_RESPONDING)
+ {
+ return;
+ }
+ if (msg.request_id != this->requested_id)
+ {
+ return;
+ }
+ auto j = nlohmann::json::parse(msg.json_msg, nullptr, false);
+ if (j.is_discarded())
+ {
+ RCLCPP_ERROR(this->_node->get_logger(), "Invalid json in api response");
+ return;
+ }
+ // TODO(luca) exception safety for missing fields, return FAILURE if
+ // submission failed
+ if (j["success"] == false)
+ {
+ RCLCPP_ERROR(this->_node->get_logger(), "Task submission failed");
+ return;
+ }
+ // Task cancellations don't have a state field
+ if (!j.contains("state"))
+ return;
+ this->rmf_task_id = j["state"]["booking"]["id"];
+}
+
+BT::NodeStatus DispatchRmfRequest::onRunning()
+{
+ if (rmf_task_id.has_value())
+ {
+ this->setOutput("rmf_task_id", *rmf_task_id);
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus ExtractDestinations::tick()
+{
+ const auto ctx = this->_ctx_mgr->current_context();
+ const auto& task = ctx->task;
+ std::deque destinations;
+ for (const auto& node : task.data)
+ {
+ if (node["type"] && node["destination"])
+ {
+ auto type = node["type"].as();
+ auto destination = node["destination"].as();
+ destinations.push_back(AmrDestination {type, destination});
+ }
+ else
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(),
+ "Order element did not contain \"type\" and \"destination\" fields");
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+ this->setOutput("destinations", destinations);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus UnpackDestinationData::tick()
+{
+ const auto destination = this->getInput("destination");
+ if (!destination)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [destination] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+ this->setOutput("workcell", destination->workcell);
+ this->setOutput("type", destination->action);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus SignalAmr::tick()
+{
+ const auto workcell = this->getInput("workcell");
+ if (!workcell)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [workcell] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+ const auto rmf_task_id = this->getInput("rmf_task_id");
+ if (!rmf_task_id)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [rmf_task_id] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+ this->_dispenser_result_pub = this->_node->create_publisher("/dispenser_results", 10);
+
+ DispenserResult msg;
+ msg.request_guid = *rmf_task_id;
+ msg.source_guid = *workcell;
+ msg.status = DispenserResult::SUCCESS;
+ this->_dispenser_result_pub->publish(msg);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus LoopDestinations::tick()
+{
+ if (!this->_initialized)
+ {
+ auto queue = this->getInput>("queue");
+ if (!queue)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [queue] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+ this->_queue = *queue;
+ this->_initialized = true;
+ }
+
+ if (this->_queue.size() == 0)
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ this->setStatus(BT::NodeStatus::RUNNING);
+
+ while (this->_queue.size() > 0)
+ {
+ RCLCPP_INFO(
+ this->_node->get_logger(), "%s: [queue] has %u items",
+ this->name().c_str(), this->_queue.size());
+ auto current_value = this->_queue.front();
+ this->setOutput("value", current_value);
+
+ this->setStatus(BT::NodeStatus::RUNNING);
+ auto child_state = this->child_node_->executeTick();
+ switch (child_state)
+ {
+ case BT::NodeStatus::SUCCESS:
+ this->haltChild();
+ this->_queue.pop_front();
+ break;
+ case BT::NodeStatus::RUNNING:
+ return BT::NodeStatus::RUNNING;
+ case BT::NodeStatus::FAILURE:
+ this->haltChild();
+ return BT::NodeStatus::FAILURE;
+ case BT::NodeStatus::IDLE:
+ throw BT::LogicError("A child node must never return IDLE");
+ }
+ }
+
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus WaitForAmr::onStart()
+{
+ const auto workcell = this->getInput("workcell");
+ if (!workcell)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [workcell] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+ const auto rmf_task_id = this->getInput("rmf_task_id");
+ if (!rmf_task_id)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: [rmf_task_id] port is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ this->_workcell = *workcell;
+ this->_rmf_task_id = *rmf_task_id;
+
+ this->_dispenser_request_sub = this->_node->create_subscription("/dispenser_requests", 10,
+ [&](DispenserRequest::UniquePtr msg)
+ {
+ this->dispenser_request_cb(*msg);
+ });
+ return BT::NodeStatus::RUNNING;
+}
+
+void WaitForAmr::dispenser_request_cb(const DispenserRequest& msg)
+{
+ if (msg.request_guid == this->_rmf_task_id &&
+ msg.target_guid == this->_workcell)
+ {
+ this->_amr_ready = true;
+ }
+}
+
+BT::NodeStatus WaitForAmr::onRunning()
+{
+ RCLCPP_INFO(
+ this->_node->get_logger(), "%s: Waiting for AMR to reach %s",
+ this->name().c_str(), this->_workcell.c_str());
+ if (this->_amr_ready)
+ {
+ this->_amr_ready = false;
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus SendSignal::tick()
+{
+ const auto ctx = this->_ctx_mgr->current_context();
+ auto signal = this->getInput("signal");
+ if (!signal)
+ {
+ RCLCPP_ERROR(
+ this->_node->get_logger(), "%s: port [signal] is required",
+ this->name().c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ this->_signal_client = std::make_unique>(
+ this->_node, endpoints::SignalWorkcellService::service_name("system_orchestrator"));
+
+ auto req =
+ std::make_shared();
+ req->task_id = ctx->task.id;
+ req->signal = *signal;
+ auto resp = this->_signal_client->send_request(req);
+ if (!resp->success)
+ {
+ // TODO(luca) implement a queueing mechanism if the request fails?
+ RCLCPP_WARN(
+ this->_node->get_logger(),
+ "%s: System orchestrator is not able to accept [%s] signal now. Skipping signaling, error: [%s]",
+ this->_node->get_name(), signal->c_str(), resp->message.c_str());
+ }
+ return BT::NodeStatus::SUCCESS;
+}
+
+}
diff --git a/nexus_capabilities/src/capabilities/transport_amr.hpp b/nexus_capabilities/src/capabilities/transport_amr.hpp
new file mode 100644
index 0000000..4c2297b
--- /dev/null
+++ b/nexus_capabilities/src/capabilities/transport_amr.hpp
@@ -0,0 +1,252 @@
+/*
+ * Copyright (C) 2023 Johnson & Johnson
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef NEXUS_WORKCELL_ORCHESTRATOR__TRANSPORT_AMR_HPP
+#define NEXUS_WORKCELL_ORCHESTRATOR__TRANSPORT_AMR_HPP
+
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+
+namespace nexus::capabilities {
+
+struct AmrDestination {
+ // Either "pickup" or "dropoff"
+ std::string action;
+ std::string workcell;
+};
+
+/**
+ * Returns RUNNING until a signal is received from system orchestrator.
+ *
+ * Input Ports:
+ * 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
+{
+// 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>("destinations", "Destinations to visit"),
+ BT::OutputPort("rmf_task_id", "The resulting RMF task id")
+ };
+ }
+
+public: DispatchRmfRequest(const std::string& name,
+ const BT::NodeConfiguration& config,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::StatefulActionNode(name, config), _node(std::move(node)) {}
+
+public: BT::NodeStatus onStart() override;
+
+public: BT::NodeStatus onRunning() override;
+
+public: void onHalted() override {}
+
+private: void submit_itinerary(const std::deque& destinations);
+
+private: void api_response_cb(const ApiResponse& msg);
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+
+private: rclcpp::Publisher::SharedPtr _api_request_pub;
+private: rclcpp::Subscription::SharedPtr _api_response_sub;
+
+// Populated by the API response callback
+private: std::optional rmf_task_id;
+private: std::string requested_id;
+};
+
+// Parses the current task in the context and returns a queue of destinations to visit
+class ExtractDestinations : public BT::SyncActionNode
+{
+public: static BT::PortsList providedPorts()
+ {
+ return {
+ BT::OutputPort>("destinations"),
+ };
+ }
+
+public: BT::NodeStatus tick() override;
+
+public: ExtractDestinations(const std::string& name,
+ const BT::NodeConfiguration& config,
+ std::shared_ptr ctx_mgr,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::SyncActionNode(name, config), _node(node), _ctx_mgr(ctx_mgr) {}
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+private: std::shared_ptr _ctx_mgr;
+};
+
+class UnpackDestinationData : public BT::SyncActionNode
+{
+public: static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort("destination"),
+ BT::OutputPort("workcell"),
+ BT::OutputPort("type"),
+ };
+ }
+
+public: UnpackDestinationData(const std::string& name,
+ const BT::NodeConfiguration& config,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::SyncActionNode(name, config), _node(node) {}
+
+public: BT::NodeStatus tick() override;
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+};
+
+// TODO(luca) consider implementing ingestors as well, will need a new input for action type
+// Sends a DispenserResult to the AMR to signal that the workcell is done
+class SignalAmr : public BT::SyncActionNode
+{
+public: using DispenserResult = rmf_dispenser_msgs::msg::DispenserResult;
+public: static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort("workcell"),
+ BT::InputPort("rmf_task_id"),
+ };
+ }
+
+public: SignalAmr(const std::string& name,
+ const BT::NodeConfiguration& config,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::SyncActionNode(name, config), _node(node) {}
+
+public: BT::NodeStatus tick() override;
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+private: rclcpp::Publisher::SharedPtr _dispenser_result_pub;
+};
+
+// Loops over destinations to extract them
+// TODO(luca) use builtin LoopNode when migrating to behaviortreecpp v4
+class LoopDestinations : public BT::DecoratorNode
+{
+ /**
+ * Defines provided ports for the BT node.
+ */
+public: static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort>("queue"),
+ BT::OutputPort("value"),
+ };
+ }
+
+/**
+ * @param name name of the BT node.
+ * @param config config of the BT node.
+ */
+public: LoopDestinations(const std::string& name,
+ const BT::NodeConfiguration& config,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::DecoratorNode{name, config}, _node(node) {}
+
+public: BT::NodeStatus tick() override;
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+private: bool _initialized = false;
+private: std::deque _queue;
+};
+
+class WaitForAmr: public BT::StatefulActionNode
+{
+// RMF interfaces
+public: using DispenserRequest = rmf_dispenser_msgs::msg::DispenserRequest;
+
+public: static BT::PortsList providedPorts()
+ {
+ return { BT::InputPort("rmf_task_id"),
+ BT::InputPort("workcell")
+ };
+ }
+
+public: WaitForAmr(const std::string& name,
+ const BT::NodeConfiguration& config,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::StatefulActionNode(name, config), _node(std::move(node)) {}
+
+public: BT::NodeStatus onStart() override;
+
+public: BT::NodeStatus onRunning() override;
+
+public: void onHalted() override {}
+
+private: void dispenser_request_cb(const DispenserRequest& msg);
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+
+private: rclcpp::Subscription::SharedPtr _dispenser_request_sub;
+
+private: std::string _rmf_task_id;
+private: std::string _workcell;
+private: bool _amr_ready = false;
+};
+
+/**
+ * Sends a signal to the system orchestrator.
+ * The signal will be tied to the current task id
+ *
+ * Input Ports:
+ * signal |std::string| Signal to send.
+ */
+class SendSignal : public BT::SyncActionNode
+{
+public: using WorkcellTask = nexus_orchestrator_msgs::msg::WorkcellTask;
+public: static BT::PortsList providedPorts()
+ {
+ return {BT::InputPort("signal", "Signal to send.") };
+ }
+
+public: SendSignal(const std::string& name, const BT::NodeConfiguration& config,
+ std::shared_ptr ctx_mgr,
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+ : BT::SyncActionNode(name, config), _node(node), _ctx_mgr(ctx_mgr) {}
+
+public: BT::NodeStatus tick() override;
+
+private: std::unique_ptr>
+ _signal_client;
+
+private: rclcpp_lifecycle::LifecycleNode::SharedPtr _node;
+private: std::shared_ptr _ctx_mgr;
+};
+
+}
+
+#endif
diff --git a/nexus_capabilities/src/capabilities/transport_amr_capability.cpp b/nexus_capabilities/src/capabilities/transport_amr_capability.cpp
new file mode 100644
index 0000000..a2981f8
--- /dev/null
+++ b/nexus_capabilities/src/capabilities/transport_amr_capability.cpp
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2022 Johnson & Johnson
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+// #include
+
+#include "transport_amr.hpp"
+#include "transport_amr_capability.hpp"
+
+namespace nexus::capabilities {
+
+void TransportAmrCapability::configure(
+ rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+ std::shared_ptr ctx_mgr,
+ BT::BehaviorTreeFactory& bt_factory)
+{
+ bt_factory.registerBuilder("transport_amr.DispatchRmfRequest",
+ [this, node](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, node);
+ });
+
+ bt_factory.registerBuilder("transport_amr.ExtractDestinations",
+ [this, node, ctx_mgr](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, ctx_mgr, node);
+ });
+
+ bt_factory.registerBuilder("transport_amr.UnpackDestinationData",
+ [this, node](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, node);
+ });
+
+ bt_factory.registerBuilder("transport_amr.SignalAmr",
+ [this, node](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, node);
+ });
+
+ bt_factory.registerBuilder("transport_amr.LoopDestinations",
+ [this, node](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, node);
+ });
+
+ bt_factory.registerBuilder("transport_amr.WaitForAmr",
+ [this, node](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, node);
+ });
+
+ bt_factory.registerBuilder("transport_amr.SendSignal",
+ [this, node, ctx_mgr](const std::string& name,
+ const BT::NodeConfiguration& config)
+ {
+ return std::make_unique(name, config, ctx_mgr, node);
+ });
+}
+
+}
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(
+ nexus::capabilities::TransportAmrCapability,
+ nexus::Capability
+)
diff --git a/nexus_capabilities/src/capabilities/transport_amr_capability.hpp b/nexus_capabilities/src/capabilities/transport_amr_capability.hpp
new file mode 100644
index 0000000..5641004
--- /dev/null
+++ b/nexus_capabilities/src/capabilities/transport_amr_capability.hpp
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2022 Johnson & Johnson
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef NEXUS_CAPABILITIES__CAPABILITIES__PLAN_MOTION_CAPABILITY_HPP
+#define NEXUS_CAPABILITIES__CAPABILITIES__PLAN_MOTION_CAPABILITY_HPP
+
+#include
+#include
+#include
+
+#include
+
+#include
+
+#include
+
+namespace nexus::capabilities {
+
+/**
+ * Capability to plan robot arm motions.
+ */
+class TransportAmrCapability : public Capability
+{
+ /**
+ * @copydoc Capability::declare_params
+ */
+public: void declare_params(rclcpp_lifecycle::LifecycleNode& /* node */) final
+ {
+ }
+
+ /**
+ * @copydoc Capability::configure
+ */
+public: void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+ std::shared_ptr ctx_mgr,
+ BT::BehaviorTreeFactory& bt_factory) final;
+
+ /**
+ * @copydoc Capability::activate
+ */
+public: void activate() final {}
+
+ /**
+ * @copydoc Capability::deactivate
+ */
+public: void deactivate() final {}
+
+private: rclcpp::Client::
+ SharedPtr _client;
+};
+
+}
+
+#endif
diff --git a/nexus_integration_tests/config/rmf_bts/transportation.xml b/nexus_integration_tests/config/rmf_bts/transportation.xml
new file mode 100644
index 0000000..f74cc64
--- /dev/null
+++ b/nexus_integration_tests/config/rmf_bts/transportation.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py
index 63170ac..782b050 100644
--- a/nexus_integration_tests/launch/control_center.launch.py
+++ b/nexus_integration_tests/launch/control_center.launch.py
@@ -120,9 +120,15 @@ def launch_setup(context, *args, **kwargs):
rmf_transporter_node = LifecycleNode(
name="rmf_nexus_transporter",
namespace="",
- package="rmf_nexus_transporter",
- executable="rmf_nexus_transporter",
- parameters=[],
+ package="nexus_workcell_orchestrator",
+ executable="nexus_workcell_orchestrator",
+ parameters=[
+ {
+ "capabilities": ["nexus::capabilities::TransportAmrCapability"],
+ "bt_path": (FindPackageShare("nexus_integration_tests"), "/config/rmf_bts"),
+ }
+ ],
+ arguments=['--ros-args', '--log-level', 'info'],
)
system_orchestrator_node = LifecycleNode(
@@ -177,7 +183,7 @@ def launch_setup(context, *args, **kwargs):
return [
SetEnvironmentVariable("ROS_DOMAIN_ID", ros_domain_id),
system_orchestrator_node,
- # rmf_transporter_node,
+ rmf_transporter_node,
transporter_node,
mock_emergency_alarm_node,
nexus_panel,