From 89203ff9676756a4431b6170c9b86a163ae6c77e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 11 Dec 2024 11:27:04 +0800 Subject: [PATCH] Add TransportAmr capability and RMF workcell Signed-off-by: Luca Della Vedova --- nexus_capabilities/CMakeLists.txt | 9 + .../src/capabilities/plugins.xml | 4 + .../src/capabilities/transport_amr.cpp | 336 ++++++++++++++++++ .../src/capabilities/transport_amr.hpp | 252 +++++++++++++ .../capabilities/transport_amr_capability.cpp | 87 +++++ .../capabilities/transport_amr_capability.hpp | 68 ++++ .../config/rmf_bts/transportation.xml | 25 ++ .../launch/control_center.launch.py | 14 +- 8 files changed, 791 insertions(+), 4 deletions(-) create mode 100644 nexus_capabilities/src/capabilities/transport_amr.cpp create mode 100644 nexus_capabilities/src/capabilities/transport_amr.hpp create mode 100644 nexus_capabilities/src/capabilities/transport_amr_capability.cpp create mode 100644 nexus_capabilities/src/capabilities/transport_amr_capability.hpp create mode 100644 nexus_integration_tests/config/rmf_bts/transportation.xml diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt index 6f9d4a3..9ceee06 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}) @@ -126,6 +129,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) @@ -135,9 +140,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,