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