diff --git a/nexus_capabilities/CMakeLists.txt b/nexus_capabilities/CMakeLists.txt index 063bf86..2a5b15c 100644 --- a/nexus_capabilities/CMakeLists.txt +++ b/nexus_capabilities/CMakeLists.txt @@ -15,8 +15,12 @@ set(dep_pkgs nexus_common nexus_endpoints nexus_orchestrator_msgs + nlohmann_json rclcpp rclcpp_lifecycle + rmf_dispenser_msgs + rmf_task_msgs + yaml_cpp_vendor yaml-cpp ) foreach(pkg ${dep_pkgs}) @@ -80,7 +84,7 @@ target_link_libraries(${PROJECT_NAME} BT::behaviortree_cpp_v3 rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle - yaml-cpp + yaml-cpp::yaml-cpp ) target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) @@ -126,6 +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/rmf_request.cpp + src/capabilities/rmf_request_capability.cpp ) target_compile_options(nexus_builtin_capabilities PUBLIC INTERFACE cxx_std_17) @@ -135,9 +141,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/package.xml b/nexus_capabilities/package.xml index 0ab7e9e..19c6a41 100644 --- a/nexus_capabilities/package.xml +++ b/nexus_capabilities/package.xml @@ -13,13 +13,17 @@ nexus_common nexus_endpoints nexus_orchestrator_msgs + nlohmann-json-dev pluginlib rclcpp rclcpp_lifecycle + rmf_dispenser_msgs + rmf_task_msgs rmf_utils tf2 tf2_ros yaml-cpp + yaml_cpp_vendor ament_cmake diff --git a/nexus_capabilities/src/capabilities/plugins.xml b/nexus_capabilities/src/capabilities/plugins.xml index c2b87bf..85fe2c8 100644 --- a/nexus_capabilities/src/capabilities/plugins.xml +++ b/nexus_capabilities/src/capabilities/plugins.xml @@ -18,4 +18,8 @@ PlanMotion capability. + + + RMFRequest capability. + diff --git a/nexus_capabilities/src/capabilities/rmf_request.cpp b/nexus_capabilities/src/capabilities/rmf_request.cpp new file mode 100644 index 0000000..b77c3c1 --- /dev/null +++ b/nexus_capabilities/src/capabilities/rmf_request.cpp @@ -0,0 +1,354 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * 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 "rmf_request.hpp" + +#include +#include + +namespace nexus::capabilities { + +namespace rmf { + +BT::NodeStatus DispatchRequest::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 DispatchRequest::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 DispatchRequest::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 DispatchRequest::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"] && node["workcell_task_id"]) + { + auto type = node["type"].as(); + auto destination = node["destination"].as(); + auto workcell_task_id = node["workcell_task_id"].as(); + destinations.push_back(Destination {type, destination, workcell_task_id}); + } + 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); + this->setOutput("workcell_task_id", destination->workcell_task_id); + 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) + { + 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->_amr_ready = false; + + 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() +{ + 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; + } + + auto workcell = this->getInput("workcell"); + if (!workcell) + { + RCLCPP_ERROR( + this->_node->get_logger(), "%s: port [workcell] is required", + this->name().c_str()); + return BT::NodeStatus::FAILURE; + } + + auto workcell_task_id = this->getInput("workcell_task_id"); + if (!workcell_task_id) + { + RCLCPP_ERROR( + this->_node->get_logger(), "%s: port [workcell_task_id] is required", + this->name().c_str()); + return BT::NodeStatus::FAILURE; + } + + this->_signal_client = std::make_unique>( + this->_node, endpoints::SignalWorkcellService::service_name(*workcell)); + + auto req = + std::make_shared(); + req->task_id = *workcell_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: Workcell [%s] is not able to accept [%s] signal now. Skipping signaling, error: [%s]", + this->_node->get_name(), workcell->c_str(), signal->c_str(), resp->message.c_str()); + } + return BT::NodeStatus::SUCCESS; +} + +} +} diff --git a/nexus_capabilities/src/capabilities/rmf_request.hpp b/nexus_capabilities/src/capabilities/rmf_request.hpp new file mode 100644 index 0000000..f0a8fb9 --- /dev/null +++ b/nexus_capabilities/src/capabilities/rmf_request.hpp @@ -0,0 +1,263 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * 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__RMF_REQUEST_HPP +#define NEXUS_WORKCELL_ORCHESTRATOR__RMF_REQUEST_HPP + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace nexus::capabilities { + +namespace rmf { + +struct Destination { + // Either "pickup" or "dropoff" + std::string action; + // Workcell to deliver to + std::string workcell; + // Task id that the workcell is executing for signaling AMR arrival + std::string workcell_task_id; +}; + +/** + * 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 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>("destinations", "Destinations to visit"), + BT::OutputPort("rmf_task_id", "The resulting RMF task id") + }; + } + +public: DispatchRequest(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"), + BT::OutputPort("workcell_task_id"), + }; + } + +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."), + BT::InputPort("workcell", "Workcell to signal."), + BT::InputPort("workcell_task_id", "Task id for the workcell to signal") + }; + } + +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/rmf_request_capability.cpp b/nexus_capabilities/src/capabilities/rmf_request_capability.cpp new file mode 100644 index 0000000..f1c1842 --- /dev/null +++ b/nexus_capabilities/src/capabilities/rmf_request_capability.cpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * 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 "rmf_request.hpp" +#include "rmf_request_capability.hpp" + +namespace nexus::capabilities { + +using namespace rmf; + +void RMFRequestCapability::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr ctx_mgr, + BT::BehaviorTreeFactory& bt_factory) +{ + bt_factory.registerBuilder("rmf_request.DispatchRequest", + [this, node](const std::string& name, + const BT::NodeConfiguration& config) + { + return std::make_unique(name, config, node); + }); + + bt_factory.registerBuilder("rmf_request.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("rmf_request.UnpackDestinationData", + [this, node](const std::string& name, + const BT::NodeConfiguration& config) + { + return std::make_unique(name, config, node); + }); + + bt_factory.registerBuilder("rmf_request.SignalAmr", + [this, node](const std::string& name, + const BT::NodeConfiguration& config) + { + return std::make_unique(name, config, node); + }); + + bt_factory.registerBuilder("rmf_request.LoopDestinations", + [this, node](const std::string& name, + const BT::NodeConfiguration& config) + { + return std::make_unique(name, config, node); + }); + + bt_factory.registerBuilder("rmf_request.WaitForAmr", + [this, node](const std::string& name, + const BT::NodeConfiguration& config) + { + return std::make_unique(name, config, node); + }); + + bt_factory.registerBuilder("rmf_request.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::RMFRequestCapability, + nexus::Capability +) diff --git a/nexus_capabilities/src/capabilities/rmf_request_capability.hpp b/nexus_capabilities/src/capabilities/rmf_request_capability.hpp new file mode 100644 index 0000000..5505a52 --- /dev/null +++ b/nexus_capabilities/src/capabilities/rmf_request_capability.hpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2025 Open Source Robotics Foundation + * + * 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__RMF_REQUEST_CAPABILITY_HPP +#define NEXUS_CAPABILITIES__CAPABILITIES__RMF_REQUEST_CAPABILITY_HPP + +#include +#include +#include + +#include + +#include + +#include + +namespace nexus::capabilities { + +/** + * Capability to plan robot arm motions. + */ +class RMFRequestCapability : 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 {} +}; + +} + +#endif diff --git a/nexus_integration_tests/CMakeLists.txt b/nexus_integration_tests/CMakeLists.txt index ea52012..e8c1518 100644 --- a/nexus_integration_tests/CMakeLists.txt +++ b/nexus_integration_tests/CMakeLists.txt @@ -176,4 +176,63 @@ if(BUILD_TESTING) endif() endif() +# Modified from rmf_demos_maps +# Get absolute path of the building map +set(building_map "config/rmf/maps/depot/depot.building.yaml") +get_filename_component(building_map_path ${building_map} REALPATH) + +# Get the output world name +string(REGEX REPLACE "\\.[^.]*\.[^.]*$" "" no_extension_path ${building_map_path}) +string(REGEX MATCH "[^\/]+$" world_name ${no_extension_path}) + +set(output_world_name ${world_name}) +set(output_dir ${CMAKE_CURRENT_BINARY_DIR}/config/rmf/maps/${output_world_name}) +set(output_world_path ${output_dir}/${output_world_name}.world) +set(output_model_dir ${output_dir}/models) + +############################################################################## +# Generate Gz world, download Models, change robot to use Fuel model +############################################################################## + +message("BUILDING WORLDFILE WITH COMMAND: ros2 run rmf_building_map_tools building_map_generator gazebo ${building_map_path} ${output_world_path} ${output_model_dir}") +message("REPLACING ROBOT MODELS WITH THOSE FROM FUEL") +if (NO_DOWNLOAD_MODELS) + add_custom_command( + DEPENDS ${building_map_path} + COMMAND ros2 run rmf_building_map_tools building_map_generator gazebo ${building_map_path} ${output_world_path} ${output_model_dir} --TEMPLATE_WORLD_FILE ${CMAKE_CURRENT_SOURCE_DIR}/config/rmf/maps/depot/depot_template.sdf --SKIP_CAMERA_POSE + COMMAND sed -i 's|model:\/\/DeliveryRobot<\/uri>|https:\/\/fuel.gazebosim.org\/1.0\/Open-RMF\/models\/DeliveryRobotWithConveyor<\/uri>|g' ${output_world_path} + OUTPUT ${output_world_path} + ) +else() + message("DOWNLOADING MODELS WITH COMMAND: ros2 run rmf_building_map_tools building_map_model_downloader ${building_map_path}") + add_custom_command( + DEPENDS ${building_map_path} + COMMAND ros2 run rmf_building_map_tools building_map_generator gazebo ${building_map_path} ${output_world_path} ${output_model_dir} --TEMPLATE_WORLD_FILE ${CMAKE_CURRENT_SOURCE_DIR}/config/rmf/maps/depot/depot_template.sdf --SKIP_CAMERA_POSE + COMMAND sed -i 's|model:\/\/DeliveryRobot<\/uri>|https:\/\/fuel.gazebosim.org\/1.0\/Open-RMF\/models\/DeliveryRobotWithConveyor<\/uri>|g' ${output_world_path} + COMMAND ros2 run rmf_building_map_tools building_map_model_downloader ${building_map_path} -e ~/.gazebo/models + OUTPUT ${output_world_path} + ) +endif() + +############################################################################## +# Generate the nav graphs +############################################################################## + +set(output_nav_graphs_dir ${output_dir}/nav_graphs/) +set(output_nav_graphs_phony ${output_nav_graphs_dir}/phony) +add_custom_command( + OUTPUT ${output_nav_graphs_phony} + COMMAND ros2 run rmf_building_map_tools building_map_generator nav ${building_map_path} ${output_nav_graphs_dir} + DEPENDS ${building_map_path} ${output_world_path} +) + +add_custom_target(generate_${output_world_name}_nav_graphs ALL + DEPENDS ${output_nav_graphs_phony} +) + +install( + DIRECTORY ${output_dir} + DESTINATION share/${PROJECT_NAME}/config/rmf/maps +) + ament_package() diff --git a/nexus_integration_tests/config/rmf/deliveryRobot_config.yaml b/nexus_integration_tests/config/rmf/deliveryRobot_config.yaml new file mode 100644 index 0000000..2fcea94 --- /dev/null +++ b/nexus_integration_tests/config/rmf/deliveryRobot_config.yaml @@ -0,0 +1,46 @@ +# FLEET CONFIG ================================================================= +# RMF Fleet parameters + +rmf_fleet: + name: "deliveryRobot" + limits: + linear: [0.7, 0.75] # velocity, acceleration + angular: [0.6, 2.0] # velocity, acceleration + profile: # Robot profile is modelled as a circle + footprint: 0.6 # radius in m + vicinity: 0.8 # radius in m + reversible: True # whether robots in this fleet can reverse + # TODO Update battery parameters with actual specs + battery_system: + voltage: 24.0 # V + capacity: 40.0 # Ahr + charging_current: 8.8 # A + mechanical_system: + mass: 70.0 # kg + moment_of_inertia: 40.0 #kgm^2 + friction_coefficient: 0.22 + ambient_system: + power: 20.0 # W + tool_system: + power: 0.0 # W + recharge_threshold: 0.10 # Battery level below which robots in this fleet will not operate + recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks + max_delay: 15.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned + publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency + account_for_battery_drain: True + task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing + loop: True + delivery: True + finishing_request: "charge" # [park, charge, nothing] + robots: + deliveryRobot1: + charger: "deliveryRobotCharger1" + deliveryRobot2: + charger: "deliveryRobotCharger2" + +fleet_manager: + ip: "127.0.0.1" + port: 22012 + user: "some_user" + password: "some_password" + robot_state_update_frequency: 10.0 diff --git a/nexus_integration_tests/config/rmf/maps/depot/depot.building.yaml b/nexus_integration_tests/config/rmf/maps/depot/depot.building.yaml new file mode 100644 index 0000000..5da70b1 --- /dev/null +++ b/nexus_integration_tests/config/rmf/maps/depot/depot.building.yaml @@ -0,0 +1,91 @@ +coordinate_system: reference_image +crowd_sim: + agent_groups: + - {agents_name: [deliveryRobot1, deliveryRobot2], agents_number: 2, group_id: 0, profile_selector: external_agent, state_selector: external_static, x: 0, y: 0} + agent_profiles: + - {ORCA_tau: 1, ORCA_tauObst: 0.40000000000000002, class: 1, max_accel: 0, max_angle_vel: 0, max_neighbors: 10, max_speed: 0, name: external_agent, neighbor_dist: 5, obstacle_set: 1, pref_speed: 0, r: 0.25} + enable: 0 + goal_sets: [] + model_types: [] + obstacle_set: {class: 1, file_name: L1_navmesh.nav, type: nav_mesh} + states: + - {final: 1, goal_set: -1, name: external_static, navmesh_file_name: ""} + transitions: [] + update_time_step: 0.10000000000000001 +graphs: + {} +levels: + L1: + drawing: + filename: depot_scan.png + elevation: 0 + lanes: + - [14, 28, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [28, 15, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [30, 16, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [31, 17, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [32, 19, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [28, 34, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [34, 33, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [34, 29, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [29, 30, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [30, 31, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [31, 32, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [32, 35, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [35, 20, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [35, 36, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [36, 21, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [36, 37, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [37, 18, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [37, 38, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [38, 22, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [38, 29, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [27, 32, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [31, 36, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + - [30, 37, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] + layers: + {} + measurements: + - [0, 1, {distance: [3, 30.199999999999999]}] + vertices: + - [0.104, 307.17000000000002, 0, ""] + - [604.029, 307.00799999999998, 0, ""] + - [2.0099999999999998, 1.994, 0, ""] + - [602.91899999999998, 1.0029999999999999, 0, ""] + - [603.96900000000005, 301.74000000000001, 0, ""] + - [1.7090000000000001, 304.33499999999998, 0, ""] + - [147.125, 72.120000000000005, 0, ""] + - [157.90799999999999, 72.030000000000001, 0, ""] + - [158.08799999999999, 82.813999999999993, 0, ""] + - [146.76499999999999, 82.903999999999996, 0, ""] + - [147.178, 222.09899999999999, 0, ""] + - [158.00399999999999, 222.04900000000001, 0, ""] + - [158.05500000000001, 233.12899999999999, 0, ""] + - [147.02600000000001, 233.02699999999999, 0, ""] + - [211.29400000000001, 284.73700000000002, 0, deliveryRobotCharger1, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, deliveryRobot1], spawn_robot_type: [1, DeliveryRobot]}] + - [240.38200000000001, 284.73700000000002, 0, deliveryRobotCharger2, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, deliveryRobot2], spawn_robot_type: [1, DeliveryRobot]}] + - [160.90700000000001, 179.114, 0, workcell_1, {dropoff_ingestor: [1, workcell_1], is_holding_point: [4, false], is_parking_spot: [4, false], pickup_dispenser: [1, workcell_1]}] + - [100.842, 179.41800000000001, 0, workcell_2, {dropoff_ingestor: [1, workcell_2], is_holding_point: [4, false], is_parking_spot: [4, false], pickup_dispenser: [1, workcell_2]}] + - [160.91300000000001, 128.69999999999999, 0, workcell_4, {dropoff_ingestor: [1, workcell_4], pickup_dispenser: [1, workcell_4]}] + - [40.776000000000003, 180.02600000000001, 0, workcell_3, {dropoff_ingestor: [1, workcell_3], pickup_dispenser: [1, workcell_3]}] + - [41.079999999999998, 128.93199999999999, 0, workcell_6, {dropoff_ingestor: [1, workcell_6], pickup_dispenser: [1, workcell_6]}] + - [101.754, 128.93199999999999, 0, workcell_5, {dropoff_ingestor: [1, workcell_5], pickup_dispenser: [1, workcell_5]}] + - [228.02099999999999, 128.25200000000001, 0, "", {is_holding_point: [4, true], is_parking_spot: [4, true]}] + - [28.983000000000001, 108.464, 0, "", {is_holding_point: [4, true], is_parking_spot: [4, true]}] + - [25.177, 123.68899999999999, 0, "", {is_holding_point: [4, true], is_parking_spot: [4, true]}] + - [20.492000000000001, 81.236000000000004, 0, "", {is_holding_point: [4, true], is_parking_spot: [4, true]}] + - [43.536999999999999, 88.259, 0, "", {is_holding_point: [4, true], is_parking_spot: [4, true]}] + - [18.771999999999998, 164.773, 0, "", {is_holding_point: [4, true], is_parking_spot: [4, true]}] + - [225.733, 250.20699999999999, 0, ""] + - [227.39699999999999, 165.74100000000001, 0, ""] + - [161.04400000000001, 165.33600000000001, 0, ""] + - [101.2, 165.006, 0, ""] + - [40.965000000000003, 164.52699999999999, 0, ""] + - [191.227, 206.31100000000001, 0, ""] + - [226.608, 206.29900000000001, 0, ""] + - [41.436, 144.524, 0, ""] + - [101.277, 144.983, 0, ""] + - [160.523, 145.184, 0, ""] + - [227.73699999999999, 145.48099999999999, 0, ""] +lifts: {} +name: depot diff --git a/nexus_integration_tests/config/rmf/maps/depot/depot_scan.png b/nexus_integration_tests/config/rmf/maps/depot/depot_scan.png new file mode 100644 index 0000000..bfc74d9 Binary files /dev/null and b/nexus_integration_tests/config/rmf/maps/depot/depot_scan.png differ diff --git a/nexus_integration_tests/config/rmf/maps/depot/depot_template.sdf b/nexus_integration_tests/config/rmf/maps/depot/depot_template.sdf new file mode 100644 index 0000000..f1fa60b --- /dev/null +++ b/nexus_integration_tests/config/rmf/maps/depot/depot_template.sdf @@ -0,0 +1,463 @@ + + + + + 0.01 + 1.0 + + + + + + + + + + 1 1 1 + 0.8 0.8 0.8 + false + + + + + + 1000 + 845 +