From a9da70a6478ecd1c0d9da49d7a41305316e318c4 Mon Sep 17 00:00:00 2001 From: Yadu Date: Mon, 28 Aug 2023 14:12:18 +0530 Subject: [PATCH 001/119] Bump main to 2.3.2 (#299) * Update CHANGELOGS Signed-off-by: Yadunund * 2.3.2 Signed-off-by: Yadunund --------- Signed-off-by: Yadunund Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/CHANGELOG.rst | 6 ++++++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 5 +++++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 5 +++++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 7 +++++++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 3 +++ rmf_websocket/package.xml | 2 +- 10 files changed, 31 insertions(+), 5 deletions(-) diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index e84b1ecda..f632aeb22 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.2 (2023-08-28) +------------------ +* Improve linking time (`#297 `_) +* EasyFullControl API (`#235 `_) +* Contributors: Grey, Luca Della Vedova, Xiyu, Yadunund + 2.3.1 (2023-08-10) ------------------ * Remove duplicate task schemas (`#294 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 31965f04a..bd99e4cc4 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.3.1 + 2.3.2 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index 326d954a3..5b4506f5a 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.2 (2023-08-28) +------------------ +* EasyFullControl API (`#235 `_) +* Contributors: Grey, Luca Della Vedova, Xiyu, Yadunund + 2.3.1 (2023-08-10) ------------------ * Adding initiator and request time to booking (`#267 `_) diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index a9086da81..b737558a4 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.3.1 + 2.3.2 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index 8cef11293..df19cbe15 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.2 (2023-08-28) +------------------ +* Improve linking time (`#297 `_) +* Contributors: Grey, Luca Della Vedova + 2.3.1 (2023-08-10) ------------------ * Adding initiator and request time to booking (`#267 `_) diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 5b41c7474..818684d7d 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.3.1 + 2.3.2 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index 727bd7b5c..8eda4437f 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.2 (2023-08-28) +------------------ +* Improve linking time (`#297 `_) +* EasyFullControl API (`#235 `_) +* Remove a few warnings related to RCLCPP logger (`#296 `_) +* Contributors: Arjo Chakravarty, Grey, Luca Della Vedova, Xiyu, Yadunund + 2.3.1 (2023-08-10) ------------------ diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 4a96ddce6..cf5e3e29e 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.3.1 + 2.3.2 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index 3101241e0..6f1d57a61 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.2 (2023-08-28) +------------------ + 2.3.1 (2023-08-10) ------------------ diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index 1f3239f2c..979a0771c 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.3.1 + 2.3.2 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From 8a71324f3e1a3e6fae1e0adef22372c510d0649a Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 28 Sep 2023 15:18:03 +0800 Subject: [PATCH 002/119] Cancel automatic pending tasks that are removed during new assignments (#301) * Cancel automatic pending tasks that are removed during new assignments Signed-off-by: Aaron Chong * Lint Signed-off-by: Aaron Chong * Use unordered_set Signed-off-by: Aaron Chong --------- Signed-off-by: Aaron Chong Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/TaskManager.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 430cacfdf..cbda9b141 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -920,6 +920,29 @@ void TaskManager::set_queue( { return; } + + // Cancel pending automatic tasks that will be removed from the new + // assignments. + std::unordered_set new_automatic_task_ids; + const std::vector cancellation_labels = + {"New task assignments received."}; + for (const auto& a : assignments) + { + if (a.request()->booking()->automatic()) + { + new_automatic_task_ids.insert(a.request()->booking()->id()); + } + } + for (const auto& a : _queue) + { + if (a.request()->booking()->automatic() && + new_automatic_task_ids.find(a.request()->booking()->id()) == + new_automatic_task_ids.end()) + { + _publish_canceled_pending_task(a, cancellation_labels); + } + } + _queue = assignments; _publish_task_queue(); } From 480f439b7fe4cfc47e461c22b331fe4fc77bbb93 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Nov 2023 11:07:10 +0800 Subject: [PATCH 003/119] Add support for multiple destinations in `GoToPlace` This commit adds support for selecting the nearest spot on the same floor. This behaviour is convenient when looking at things from a cancellation perspective. This commit depends on https://github.com/open-rmf/rmf_task/pull/101 Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 114 +++++++++++++++--- .../rmf_fleet_adapter/events/GoToPlace.hpp | 4 +- .../src/rmf_fleet_adapter/tasks/Patrol.cpp | 20 +++ 3 files changed, 119 insertions(+), 19 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index c3b465641..aca7f9e57 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -19,6 +19,7 @@ #include "../project_itinerary.hpp" #include +#include namespace rmf_fleet_adapter { namespace events { @@ -67,7 +68,7 @@ auto GoToPlace::Standby::make( const auto context = state.get()->value; const auto header = description.generate_header(state, *parameters); - auto standby = std::make_shared(Standby{description.destination()}); + auto standby = std::make_shared(Standby{description.destinations()}); standby->_followed_by = description.expected_next_destinations(); standby->_assign_id = id; standby->_context = context; @@ -86,7 +87,7 @@ auto GoToPlace::Standby::make( } //============================================================================== -GoToPlace::Standby::Standby(rmf_traffic::agv::Plan::Goal goal) +GoToPlace::Standby::Standby(std::vector goal) : _goal(std::move(goal)) { // Do nothin @@ -111,21 +112,100 @@ auto GoToPlace::Standby::begin( { if (!_active) { - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning a new go_to_place [%lu] for robot [%s]", - _goal.waypoint(), - _context->requester_id().c_str()); - - _active = Active::make( - _assign_id, - _context, - _goal, - _followed_by, - _tail_period, - _state, - _update, - std::move(finished)); + if (_goal.size() == 1) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning a new go_to_place [%lu] for robot [%s]", + _goal[0].waypoint(), + _context->requester_id().c_str()); + + _active = Active::make( + _assign_id, + _context, + _goal[0], + _followed_by, + _tail_period, + _state, + _update, + std::move(finished)); + } + else { + // Pick nearest parking spot based on distance + // and being on same floor. + auto current_location = _context->location(); + auto graph = _context->navigation_graph(); + if (current_location.size() ==0) { + //unable to get location + RCLCPP_ERROR(_context->node()->get_logger(), + "Robot [%s] can't get location", + _context->requester_id().c_str()); + } + RCLCPP_INFO( + _context->node()->get_logger(), + "Selecting a new go_to_place location from [%lu] choices for robot [%s]", + _goal.size(), + _context->requester_id().c_str()); + auto lowest_cost = std::numeric_limits::infinity(); + std::size_t selected_idx = 0; + for (std::size_t i = 0; i < _goal.size(); i++) { + auto wp_idx = _goal[i].waypoint(); + auto wp = graph.get_waypoint(wp_idx); + + // Check if same map. If not don't consider location. This is to ensure the + // robot does not try to board a lift. + if (wp.get_map_name() != _context->map()) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + _context->map().c_str() + ); + continue; + } + + // Find distance to said point + auto result = _context->planner()->quickest_path( + current_location, wp_idx); + + if (result.has_value()) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Got distance from [%lu] as %f", + wp_idx, + result->cost() + ); + if (result->cost() < lowest_cost) { + selected_idx = i; + lowest_cost = result->cost(); + } + } + else { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + wp_idx, + _context->requester_id().c_str() + ); + } + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning a new go_to_place [%lu] for robot [%s]", + _goal[selected_idx].waypoint(), + _context->requester_id().c_str()); + + _active = Active::make( + _assign_id, + _context, + _goal[selected_idx], + _followed_by, + _tail_period, + _state, + _update, + std::move(finished)); + } } return _active; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 89eb829ee..9d9cc882e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -63,9 +63,9 @@ class GoToPlace : public rmf_task_sequence::Event private: - Standby(rmf_traffic::agv::Plan::Goal goal); + Standby(std::vector goal); - rmf_traffic::agv::Plan::Goal _goal; + std::vector _goal; std::vector _followed_by; AssignIDPtr _assign_id; agv::RobotContextPtr _context; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp index 75cefef9f..ff7931574 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp @@ -50,6 +50,26 @@ void add_patrol( -> agv::DeserializedEvent { nlohmann::json place_msg; + const auto nearest_place = msg.find("nearest_of"); + if (nearest_place != msg.end()) { + std::vector goals; + std::vector errors; + for (auto &place_msg: nearest_place.value()) { + auto place = place_deser(place_msg); + if (!place.description.has_value()) + { + return {nullptr, std::move(place.errors)}; + } + + goals.push_back(*place.description); + errors.insert( + errors.end(), + std::make_move_iterator(place.errors.begin()), + std::make_move_iterator(place.errors.end())); + } + auto desc = GoToPlace::Description::make_with_multiple(goals); + return {desc, errors}; + } const auto place_it = msg.find("place"); if (place_it == msg.end()) place_msg = msg; From 267d20057650b98529e53a2c3beb487833c34b5d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Nov 2023 11:24:49 +0800 Subject: [PATCH 004/119] Missed the schema Signed-off-by: Arjo Chakravarty --- .../schemas/event_description__go_to_place.json | 10 ++++++++++ .../src/rmf_fleet_adapter/events/GoToPlace.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/schemas/event_description__go_to_place.json b/rmf_fleet_adapter/schemas/event_description__go_to_place.json index 8d932d01f..0d3ae2c98 100644 --- a/rmf_fleet_adapter/schemas/event_description__go_to_place.json +++ b/rmf_fleet_adapter/schemas/event_description__go_to_place.json @@ -18,6 +18,16 @@ } }, "required": ["place"] + }, + { + "type": "object", + "properties" : { + "nearest_of" : { + "type": "array", + "description": "A list of places to choose from. Will try to go to the nearest one.", + "items": { "$ref": "place.json" } + } + } } ] } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index aca7f9e57..6ad79331b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -135,7 +135,7 @@ auto GoToPlace::Standby::begin( auto current_location = _context->location(); auto graph = _context->navigation_graph(); if (current_location.size() ==0) { - //unable to get location + //unable to get location. We should return some form of error stste. RCLCPP_ERROR(_context->node()->get_logger(), "Robot [%s] can't get location", _context->requester_id().c_str()); From a3166758859ed1f30cccffd7cada7d449f5da6bf Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 21 Nov 2023 13:19:58 +0800 Subject: [PATCH 005/119] Update schema to follow @mxgrey's specifications. I have not implemented flexible constraints here. If we really need it can do it in a follow up pr. Signed-off-by: Arjo Chakravarty --- .../event_description__go_to_place.json | 22 +++++++++++++++++-- .../src/rmf_fleet_adapter/tasks/Patrol.cpp | 2 +- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/schemas/event_description__go_to_place.json b/rmf_fleet_adapter/schemas/event_description__go_to_place.json index 0d3ae2c98..ad9cde460 100644 --- a/rmf_fleet_adapter/schemas/event_description__go_to_place.json +++ b/rmf_fleet_adapter/schemas/event_description__go_to_place.json @@ -22,12 +22,30 @@ { "type": "object", "properties" : { - "nearest_of" : { + "one_of" : { "type": "array", "description": "A list of places to choose from. Will try to go to the nearest one.", "items": { "$ref": "place.json" } + }, + "constraints" : { + "type": "array", + "description": "list of constraints to use", + "items": {"$ref": "#/$defs/constraint"} + } + } + } + ], + "$defs": { + "constraint": { + "type": "object", + "properties": { + "category": { + "type": "string" + }, + "description": { + "type": "string" } } } - ] + } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp index ff7931574..9db893090 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp @@ -50,7 +50,7 @@ void add_patrol( -> agv::DeserializedEvent { nlohmann::json place_msg; - const auto nearest_place = msg.find("nearest_of"); + const auto nearest_place = msg.find("one_of"); if (nearest_place != msg.end()) { std::vector goals; std::vector errors; From 6dda0724d65dd751dfdcdb74076697918db259b4 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 18 Dec 2023 22:41:11 +0800 Subject: [PATCH 006/119] Tweaking implementation Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../event_description__go_to_place.json | 14 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 280 +++++++++++------- .../rmf_fleet_adapter/events/GoToPlace.hpp | 19 +- .../events/ResponsiveWait.cpp | 3 +- .../src/rmf_fleet_adapter/tasks/Patrol.cpp | 22 +- 5 files changed, 205 insertions(+), 133 deletions(-) diff --git a/rmf_fleet_adapter/schemas/event_description__go_to_place.json b/rmf_fleet_adapter/schemas/event_description__go_to_place.json index ad9cde460..2d78a2070 100644 --- a/rmf_fleet_adapter/schemas/event_description__go_to_place.json +++ b/rmf_fleet_adapter/schemas/event_description__go_to_place.json @@ -25,14 +25,16 @@ "one_of" : { "type": "array", "description": "A list of places to choose from. Will try to go to the nearest one.", - "items": { "$ref": "place.json" } + "items": { "$ref": "place.json" }, + "minItems": 1 }, "constraints" : { "type": "array", "description": "list of constraints to use", "items": {"$ref": "#/$defs/constraint"} } - } + }, + "required": ["one_of"] } ], "$defs": { @@ -40,12 +42,14 @@ "type": "object", "properties": { "category": { - "type": "string" + "type": "string", + "description": "The type of constraint, e.g. on_same_map" }, "description": { - "type": "string" + "description": "A complete description of the constraint" } - } + }, + "required": ["category"] } } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 6ad79331b..08025eb9b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -68,8 +68,7 @@ auto GoToPlace::Standby::make( const auto context = state.get()->value; const auto header = description.generate_header(state, *parameters); - auto standby = std::make_shared(Standby{description.destinations()}); - standby->_followed_by = description.expected_next_destinations(); + auto standby = std::make_shared(Standby{description}); standby->_assign_id = id; standby->_context = context; standby->_time_estimate = header.original_duration_estimate(); @@ -87,8 +86,8 @@ auto GoToPlace::Standby::make( } //============================================================================== -GoToPlace::Standby::Standby(std::vector goal) -: _goal(std::move(goal)) +GoToPlace::Standby::Standby(Description description) +: _description(std::move(description)) { // Do nothin } @@ -112,100 +111,14 @@ auto GoToPlace::Standby::begin( { if (!_active) { - if (_goal.size() == 1) { - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning a new go_to_place [%lu] for robot [%s]", - _goal[0].waypoint(), - _context->requester_id().c_str()); - - _active = Active::make( - _assign_id, - _context, - _goal[0], - _followed_by, - _tail_period, - _state, - _update, - std::move(finished)); - } - else { - // Pick nearest parking spot based on distance - // and being on same floor. - auto current_location = _context->location(); - auto graph = _context->navigation_graph(); - if (current_location.size() ==0) { - //unable to get location. We should return some form of error stste. - RCLCPP_ERROR(_context->node()->get_logger(), - "Robot [%s] can't get location", - _context->requester_id().c_str()); - } - RCLCPP_INFO( - _context->node()->get_logger(), - "Selecting a new go_to_place location from [%lu] choices for robot [%s]", - _goal.size(), - _context->requester_id().c_str()); - auto lowest_cost = std::numeric_limits::infinity(); - std::size_t selected_idx = 0; - for (std::size_t i = 0; i < _goal.size(); i++) { - auto wp_idx = _goal[i].waypoint(); - auto wp = graph.get_waypoint(wp_idx); - - // Check if same map. If not don't consider location. This is to ensure the - // robot does not try to board a lift. - if (wp.get_map_name() != _context->map()) { - RCLCPP_INFO( - _context->node()->get_logger(), - "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", - wp_idx, - wp.get_map_name().c_str(), - _context->map().c_str() - ); - continue; - } - - // Find distance to said point - auto result = _context->planner()->quickest_path( - current_location, wp_idx); - - if (result.has_value()) { - RCLCPP_INFO( - _context->node()->get_logger(), - "Got distance from [%lu] as %f", - wp_idx, - result->cost() - ); - if (result->cost() < lowest_cost) { - selected_idx = i; - lowest_cost = result->cost(); - } - } - else { - RCLCPP_ERROR( - _context->node()->get_logger(), - "No path found for robot [%s] to waypoint [%lu]", - wp_idx, - _context->requester_id().c_str() - ); - } - } - - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning a new go_to_place [%lu] for robot [%s]", - _goal[selected_idx].waypoint(), - _context->requester_id().c_str()); - - _active = Active::make( - _assign_id, - _context, - _goal[selected_idx], - _followed_by, - _tail_period, - _state, - _update, - std::move(finished)); - } + _active = Active::make( + _assign_id, + _context, + _description, + _tail_period, + _state, + _update, + std::move(finished)); } return _active; @@ -215,21 +128,41 @@ auto GoToPlace::Standby::begin( auto GoToPlace::Active::make( const AssignIDPtr& id, agv::RobotContextPtr context, - rmf_traffic::agv::Plan::Goal goal, - std::vector followed_by, + Description description, std::optional tail_period, rmf_task::events::SimpleEventStatePtr state, std::function update, std::function finished) -> std::shared_ptr { - auto active = std::make_shared(Active(std::move(goal))); - active->_followed_by = std::move(followed_by); + auto active = std::make_shared(Active(std::move(description))); active->_assign_id = id; active->_context = std::move(context); active->_tail_period = tail_period; active->_update = std::move(update); active->_finished = std::move(finished); active->_state = std::move(state); + + if (active->_description.one_of().empty()) + { + active->_state->update_status(Status::Error); + active->_state->update_log().error( + "No destination option was provided to go_to_place. There is nowhere to " + "go, so we will proceed to the next step in the task."); + + RCLCPP_ERROR( + active->_context->node()->get_logger(), + "No destination option was provided for a go_to_place for [%s]. There is " + "nowhere to go, so we will proceed to the next step in the task.", + active->_context->requester_id().c_str()); + + active->_context->worker().schedule( + [finished = active->_finished](const auto&) + { + finished(); + }); + return active; + } + active->_negotiator = Negotiator::make( active->_context, @@ -350,8 +283,13 @@ rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const return finish - now + *delay; } + if (!_chosen_goal.has_value()) + { + return rmf_traffic::Duration(0); + } + const auto& estimate = - _context->planner()->setup(_context->location(), _goal); + _context->planner()->setup(_context->location(), _chosen_goal->waypoint()); if (estimate.ideal_cost().has_value()) return rmf_traffic::time::from_seconds(*estimate.ideal_cost()); @@ -403,8 +341,7 @@ void GoToPlace::Active::cancel() { RCLCPP_INFO( _context->node()->get_logger(), - "Canceling go_to_place [%lu] for robot [%s]", - _goal.waypoint(), + "Canceling go_to_place for robot [%s]", _context->requester_id().c_str()); _stop_and_clear(); _state->update_status(Status::Canceled); @@ -452,21 +389,127 @@ std::string wp_name(const agv::RobotContext& context) return "#" + std::to_string(locations.front().waypoint()); } +//============================================================================== +std::optional GoToPlace::Active::_choose_goal( + bool only_same_map) const +{ + auto current_location = _context->location(); + auto graph = _context->navigation_graph(); + if (current_location.size() == 0) + { + //unable to get location. We should return some form of error stste. + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] can't get location", + _context->requester_id().c_str()); + return std::nullopt; + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Selecting a new go_to_place location from [%lu] choices for robot [%s]", + _description.one_of().size(), + _context->requester_id().c_str()); + + auto lowest_cost = std::numeric_limits::infinity(); + std::optional selected_idx; + for (std::size_t i = 0; i < _description.one_of().size(); ++i) + { + const auto wp_idx = _description.one_of()[i].waypoint(); + if (only_same_map) + { + const auto& wp = graph.get_waypoint(wp_idx); + + // Check if same map. If not don't consider location. This is to ensure + // the robot does not try to board a lift. + if (wp.get_map_name() != _context->map()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + _context->map().c_str()); + continue; + } + } + + // Find distance to said point + auto result = _context->planner()->quickest_path(current_location, wp_idx); + if (result.has_value()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Got distance from [%lu] as %f", + wp_idx, + result->cost()); + + if (result->cost() < lowest_cost) + { + selected_idx = i; + lowest_cost = result->cost(); + } + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + wp_idx, + _context->requester_id().c_str()); + } + } + + if (selected_idx.has_value()) + { + return _description.one_of()[*selected_idx]; + } + + return std::nullopt; +} + //============================================================================== void GoToPlace::Active::_find_plan() { if (_is_interrupted) return; + if (!_chosen_goal.has_value() && _description.prefer_same_map()) + { + _chosen_goal = _choose_goal(true); + } + + if (!_chosen_goal.has_value()) + { + _chosen_goal = _choose_goal(false); + } + + if (!_chosen_goal.has_value()) + { + std::string error_msg = "Unable to find a path to any of the goal options [" + + _description.destination_name(*_context->task_parameters()) + + "]"; + + _state->update_log().error(error_msg); + RCLCPP_ERROR( + _context->node()->get_logger(), + "%s for [%s]", + error_msg.c_str(), + _context->requester_id().c_str()); + + _schedule_retry(); + return; + } + _state->update_status(Status::Underway); const auto start_name = wp_name(*_context); - const auto goal_name = wp_name(*_context, _goal); + const auto goal_name = wp_name(*_context, *_chosen_goal); _state->update_log().info( "Generating plan to move from [" + start_name + "] to [" + goal_name + "]"); // TODO(MXG): Make the planning time limit configurable _find_path_service = std::make_shared( - _context->planner(), _context->location(), _goal, + _context->planner(), _context->location(), *_chosen_goal, _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile(), std::chrono::seconds(5)); @@ -505,7 +548,8 @@ void GoToPlace::Active::_find_plan() + start_name + "] to [" + goal_name + "]"); auto full_itinerary = project_itinerary( - *result, self->_followed_by, *self->_context->planner()); + *result, self->_description.expected_next_destinations(), + *self->_context->planner()); self->_execute_plan( self->_context->itinerary().assign_plan_id(), @@ -534,8 +578,8 @@ void GoToPlace::Active::_find_plan() } //============================================================================== -GoToPlace::Active::Active(rmf_traffic::agv::Plan::Goal goal) -: _goal(std::move(goal)) +GoToPlace::Active::Active(Description description) +: _description(std::move(description)) { // Do nothing } @@ -581,10 +625,13 @@ void GoToPlace::Active::_execute_plan( return; } + const auto& graph = _context->navigation_graph(); + RCLCPP_INFO( _context->node()->get_logger(), - "Executing go_to_place [%lu] for robot [%s]", - _goal.waypoint(), + "Executing go_to_place [%s] for robot [%s]", + graph.get_waypoint(plan.get_waypoints().back().graph_index().value()) + .name_or_index().c_str(), _context->requester_id().c_str()); _execution = ExecutePlan::make( @@ -616,6 +663,12 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( const Negotiator::TableViewerPtr& table_view, const Negotiator::ResponderPtr& responder) { + if (!_chosen_goal.has_value()) + { + responder->forfeit({}); + return nullptr; + } + auto approval_cb = [w = weak_from_this()]( const rmf_traffic::PlanId plan_id, const rmf_traffic::agv::Plan& plan, @@ -634,7 +687,8 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( const auto evaluator = Negotiator::make_evaluator(table_view); return services::Negotiate::path( _context->itinerary().assign_plan_id(), _context->planner(), - _context->location(), _goal, _followed_by, table_view, + _context->location(), *_chosen_goal, + _description.expected_next_destinations(), table_view, responder, std::move(approval_cb), std::move(evaluator)); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 9d9cc882e..acc72f9ee 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -49,7 +49,7 @@ class GoToPlace : public rmf_task_sequence::Event const AssignIDPtr& id, const std::function& get_state, const rmf_task::ConstParametersPtr& parameters, - const rmf_task_sequence::events::GoToPlace::Description& description, + const Description& description, std::function update, std::optional tail_period = std::nullopt); @@ -63,10 +63,9 @@ class GoToPlace : public rmf_task_sequence::Event private: - Standby(std::vector goal); + Standby(Description description); - std::vector _goal; - std::vector _followed_by; + Description _description; AssignIDPtr _assign_id; agv::RobotContextPtr _context; rmf_traffic::Duration _time_estimate; @@ -85,8 +84,7 @@ class GoToPlace : public rmf_task_sequence::Event static std::shared_ptr make( const AssignIDPtr& id, agv::RobotContextPtr context, - rmf_traffic::agv::Plan::Goal goal, - std::vector followed_by, + Description description, std::optional tail_period, rmf_task::events::SimpleEventStatePtr state, std::function update, @@ -106,10 +104,13 @@ class GoToPlace : public rmf_task_sequence::Event private: - Active(rmf_traffic::agv::Plan::Goal goal); + Active(Description description); void _schedule_retry(); + std::optional _choose_goal( + bool only_same_map) const; + void _find_plan(); void _execute_plan( @@ -123,8 +124,8 @@ class GoToPlace : public rmf_task_sequence::Event const Negotiator::TableViewerPtr& table_view, const Negotiator::ResponderPtr& responder); - rmf_traffic::agv::Plan::Goal _goal; - std::vector _followed_by; + Description _description; + std::optional _chosen_goal; AssignIDPtr _assign_id; agv::RobotContextPtr _context; std::optional _tail_period; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp index 40f89104f..79c7db92a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp @@ -319,8 +319,7 @@ void ResponsiveWait::Active::_begin_movement() _go_to_place = GoToPlace::Active::make( _assign_id, _context, - std::move(goal), - {}, + *GoToPlace::Description::make(goal), _description.period, _state, _update, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp index 9db893090..c3358627e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp @@ -50,11 +50,13 @@ void add_patrol( -> agv::DeserializedEvent { nlohmann::json place_msg; - const auto nearest_place = msg.find("one_of"); - if (nearest_place != msg.end()) { + const auto one_of = msg.find("one_of"); + if (one_of != msg.end()) + { std::vector goals; std::vector errors; - for (auto &place_msg: nearest_place.value()) { + for (const auto& place_msg : one_of.value()) + { auto place = place_deser(place_msg); if (!place.description.has_value()) { @@ -67,8 +69,20 @@ void add_patrol( std::make_move_iterator(place.errors.begin()), std::make_move_iterator(place.errors.end())); } - auto desc = GoToPlace::Description::make_with_multiple(goals); + auto desc = GoToPlace::Description::make_for_one_of(goals); return {desc, errors}; + + const auto constraints = msg.find("constraints"); + if (constraints != msg.end()) + { + for (const auto& constraint : constraints.value()) + { + if (constraint["type"].get() == "prefer_same_map") + { + desc->prefer_same_map(true); + } + } + } } const auto place_it = msg.find("place"); if (place_it == msg.end()) From 3a3786e1d957e6acfd5825c4c7526ff83b91121e Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 15 Dec 2023 10:57:42 +0800 Subject: [PATCH 007/119] Mutex groups, dynamic charging, and more (#310) Signed-off-by: Michael X. Grey Signed-off-by: Xiyu Oh Co-authored-by: Xiyu Oh Signed-off-by: Arjo Chakravarty --- rmf_charging_schedule/CHANGELOG.rst | 7 + rmf_charging_schedule/README.md | 39 + rmf_charging_schedule/package.xml | 16 + .../resource/rmf_charging_schedule | 0 .../rmf_charging_schedule/__init__.py | 0 .../rmf_charging_schedule/main.py | 240 ++++ rmf_charging_schedule/setup.cfg | 4 + rmf_charging_schedule/setup.py | 25 + rmf_fleet_adapter/CMakeLists.txt | 20 + .../rmf_fleet_adapter/StandardNames.hpp | 7 + .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 66 +- .../agv/FleetUpdateHandle.hpp | 20 + .../agv/RobotUpdateHandle.hpp | 10 + .../tasks/ParkRobotIndefinitely.hpp | 62 + rmf_fleet_adapter/src/full_control/main.cpp | 4 +- .../src/lift_supervisor/Node.cpp | 38 +- .../src/mutex_group_supervisor/main.cpp | 286 +++++ .../src/rmf_fleet_adapter/LegacyTask.hpp | 2 + .../src/rmf_fleet_adapter/TaskManager.cpp | 55 +- .../src/rmf_fleet_adapter/TaskManager.hpp | 6 + .../src/rmf_fleet_adapter/agv/Adapter.cpp | 7 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 1014 ++++++++++------- .../agv/FleetUpdateHandle.cpp | 781 ++++++++----- .../src/rmf_fleet_adapter/agv/Node.cpp | 34 +- .../src/rmf_fleet_adapter/agv/Node.hpp | 16 + .../rmf_fleet_adapter/agv/RobotContext.cpp | 1001 +++++++++++++++- .../rmf_fleet_adapter/agv/RobotContext.hpp | 462 +++++++- .../agv/RobotUpdateHandle.cpp | 141 ++- .../rmf_fleet_adapter/agv/Transformation.cpp | 2 +- .../agv/internal_EasyFullControl.hpp | 65 ++ .../agv/internal_FleetUpdateHandle.hpp | 101 +- .../agv/internal_RobotUpdateHandle.hpp | 19 +- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 194 +++- .../agv/test/MockAdapter.cpp | 43 +- .../events/EmergencyPullover.cpp | 38 +- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 819 +++++++++++-- .../rmf_fleet_adapter/events/ExecutePlan.hpp | 4 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 38 +- .../events/LockMutexGroup.cpp | 401 +++++++ .../events/LockMutexGroup.hpp | 119 ++ .../events/PerformAction.cpp | 1 + .../events/WaitForCancel.cpp | 122 ++ .../events/WaitForCancel.hpp | 87 ++ .../events/WaitForTraffic.cpp | 65 +- .../events/WaitForTraffic.hpp | 6 +- .../rmf_fleet_adapter/phases/DockRobot.cpp | 40 +- .../rmf_fleet_adapter/phases/DockRobot.hpp | 85 +- .../rmf_fleet_adapter/phases/DoorClose.cpp | 6 + .../src/rmf_fleet_adapter/phases/DoorOpen.cpp | 6 + .../phases/EndLiftSession.cpp | 50 +- .../phases/EndLiftSession.hpp | 2 - .../rmf_fleet_adapter/phases/MoveRobot.cpp | 31 +- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 341 +++--- .../rmf_fleet_adapter/phases/RequestLift.cpp | 210 +++- .../rmf_fleet_adapter/phases/RequestLift.hpp | 28 +- .../phases/WaitForCharge.cpp | 37 +- .../phases/WaitForCharge.hpp | 10 +- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 623 +++++++++- .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 8 - .../test/phases/test_RequestLift.cpp | 77 +- rmf_fleet_adapter_python/src/adapter.cpp | 35 +- rmf_fleet_adapter_python/src/graph/graph.cpp | 13 +- 62 files changed, 6751 insertions(+), 1338 deletions(-) create mode 100644 rmf_charging_schedule/CHANGELOG.rst create mode 100644 rmf_charging_schedule/README.md create mode 100644 rmf_charging_schedule/package.xml create mode 100644 rmf_charging_schedule/resource/rmf_charging_schedule create mode 100644 rmf_charging_schedule/rmf_charging_schedule/__init__.py create mode 100644 rmf_charging_schedule/rmf_charging_schedule/main.py create mode 100644 rmf_charging_schedule/setup.cfg create mode 100644 rmf_charging_schedule/setup.py create mode 100644 rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp create mode 100644 rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst new file mode 100644 index 000000000..bbd8f97d9 --- /dev/null +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmf_charging_schedule +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.2 (XXXX-YY-ZZ) +------------------ +* First release diff --git a/rmf_charging_schedule/README.md b/rmf_charging_schedule/README.md new file mode 100644 index 000000000..3a14c818b --- /dev/null +++ b/rmf_charging_schedule/README.md @@ -0,0 +1,39 @@ +A simple node that takes in a yaml file that describes a schedule for charger +usage in an RMF scenario. The node will watch the clock and publish the +appropriate commands to change the chargers of the robots. + +The format for the schedule looks like this: +``` +"my_fleet_name": + "00:00": { "robot_1": "charger_A", "robot_2": "charger_B", "robot_3": "queue_A" } + "01:55": { "robot_1": "queue_B" } + "02:00": { "robot_3": "charger_A", "robot_1": "queue_A" } + "03:55": { "robot_2": "queue_B" } + "04:00": { "robot_1": "charger_B", "robot_2": "queue_A" } +``` + +The time format is `"HH:MM"` where `HH` ranges from `00` to `23` and `MM` ranges +from `00` to `59`. Note that quotes are important because otherwise the yaml +format may confuse the meaning of the colon `:`. + +The schedule will cycle every 24 hours. + +For each timestamp, only robots that are explicitly mentioned will have their +dedicated charger changed. **It is the responsibility of the schedule file author +to make sure that two robots are never assigned the same charger at the same +time.** Failing to ensure this may cause traffic and task management to misbehave. + +When run in simulation mode (`--ros-args --use-sim-time`), the time `00:00` in +the schedule will correspond to `t=0.0` in simulation time. + +When run without sim time on, the hours and minutes will correspond to the local +timezone of the machine that the node is run on. To choose a specific timezone +instead of using the system's local timzeone, use the `--timezone` argument and +provide the desired [TZ identifier](https://en.wikipedia.org/wiki/List_of_tz_database_time_zones) +string. + +It is advisable that you always put a `00:00` entry that indicates **all** of +the intended charger assignments at midnight. When the node is launched, it will +move through the schedule from the earliest entry up until the last relevant one +and issue an initial charger assignment message based on what the assignments +would have been if the schedule had run from `00:00`. diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml new file mode 100644 index 000000000..f53610f8c --- /dev/null +++ b/rmf_charging_schedule/package.xml @@ -0,0 +1,16 @@ + + + + rmf_charging_schedule + 2.3.2 + Node for a fixed 24-hour rotating charger usage schedule + Grey + Apache License 2.0 + + rclpy + rmf_fleet_msgs + + + ament_python + + diff --git a/rmf_charging_schedule/resource/rmf_charging_schedule b/rmf_charging_schedule/resource/rmf_charging_schedule new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_charging_schedule/rmf_charging_schedule/__init__.py b/rmf_charging_schedule/rmf_charging_schedule/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_charging_schedule/rmf_charging_schedule/main.py b/rmf_charging_schedule/rmf_charging_schedule/main.py new file mode 100644 index 000000000..9d90062c7 --- /dev/null +++ b/rmf_charging_schedule/rmf_charging_schedule/main.py @@ -0,0 +1,240 @@ +import sys +import yaml +import datetime +import argparse +from collections import OrderedDict +import bisect +from functools import total_ordering +from zoneinfo import ZoneInfo + +from icecream import ic + +import rclpy +from rclpy.node import Node, Publisher +from rclpy.qos import ( + QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy +) + +from rmf_fleet_msgs.msg import ChargingAssignment, ChargingAssignments + +@total_ordering +class ScheduleTimePoint: + hour: int + minute: int + + def __init__(self, hour: int, minute: int): + self.hour = hour + self.minute = minute + + def parse(text: str): + segments = text.split(":") + assert len(segments) == 2, ( + f'Time point text [{text}] does not have the correct HH:MM format' + ) + hh = int(segments[0]) + mm = int(segments[1]) + assert 0 <= hh and hh < 24, ( + f'Time point text [{text}] has an hour value which is outside the ' + f'valid range of 0 -> 23.' + ) + assert 0 <= mm and mm < 60, ( + f'Time point text [{text}] has a minute value which is outside the ' + f'valid range of 0 -> 59' + ) + return ScheduleTimePoint(hh, mm) + + def __eq__(self, other): + return self.hour == other.hour and self.minute == other.minute + + def __lt__(self, other): + if self.hour < other.hour: + return True + elif self.hour > other.hour: + return False + return self.minute < other.minute + + def __hash__(self): + return hash((self.hour, self.minute)) + + +class Assignment: + fleet: str + robot: str + charger: str + + def __init__(self, fleet, robot, charger): + self.fleet = fleet + self.robot = robot + self.charger = charger + + +def publish_assignments(publisher: Publisher, assignments: dict[dict[str]]): + for fleet, robots in assignments.items(): + msg = ChargingAssignments() + msg.fleet_name = fleet + for robot, charger in robots.items(): + assignment = ChargingAssignment() + assignment.robot_name = robot + assignment.waypoint_name = charger + # The mode isn't actually used yet, so it doesn't matter what we set + # it to. + assignment.mode = ChargingAssignment.MODE_CHARGE + msg.assignments.append(assignment) + + publisher.publish(msg) + +def update_assignments( + last_update_index: int | None, + next_update_index: int, + sorted_times: list, + schedule: dict, + assignments: dict, + publisher: Publisher, + node: Node, +): + for key in sorted_times[last_update_index:next_update_index]: + changes: list[Assignment] = schedule[key] + for change in changes: + assignments.setdefault(change.fleet, {})[change.robot] = change.charger + node.get_logger().info( + f'Sending {change.fleet}/{change.robot} to {change.charger} at ' + f'{key.hour:02d}:{key.minute:02d}' + ) + publish_assignments(publisher, assignments) + + +def simulation_time(node: Node) -> ScheduleTimePoint: + seconds, _ = node.get_clock().now().seconds_nanoseconds() + minutes: int = int(seconds/60) + hour: int = int((minutes/60) % 24) + minute = minutes % 60 + return ScheduleTimePoint(hour, minute) + + +def real_time(node: Node, timezone: ZoneInfo) -> ScheduleTimePoint: + nanoseconds = float(node.get_clock().now().nanoseconds) + seconds = nanoseconds / 1e9 + dt = datetime.datetime.fromtimestamp(seconds, timezone) + return ScheduleTimePoint(dt.hour, dt.minute) + + +def main(argv=sys.argv): + rclpy.init(args=argv) + node = Node("rmf_charging_schedule") + use_sim_time = node.get_parameter('use_sim_time').value + + args_without_ros = rclpy.utilities.remove_ros_args(argv) + + parser = argparse.ArgumentParser( + prog='rmf_charging_schedule', + description='Manage a fixed 24-hour charger schedule rotation', + ) + parser.add_argument( + 'schedule', type=str, + help=( + 'A .yaml file representing the schedule. See README for the ' + 'expected format.' + ) + ) + parser.add_argument( + '-z', '--timezone', type=str, required=False, + help=( + 'Timezone that the 24-hour rotation will be based on. If not ' + 'provided, the system\'s local timezone will be used.' + ) + ) + parser.add_argument( + '-t', '--test-time', action='store_true', + help=( + 'Use this option to test the real time calculation by printing the ' + 'current HH:MM based on your settings. This may be used in ' + 'conjunction with the --timezone option and sim time. The node ' + 'will immediately quit after printing the time, so this will not ' + 'publish any assignment messages.' + ) + ) + + args = parser.parse_args(args_without_ros[1:]) + schedule_file = args.schedule + + if args.timezone is not None: + timezone = ZoneInfo(args.timezone) + else: + timezone = None + + if use_sim_time: + get_time = lambda: simulation_time(node) + else: + get_time = lambda: real_time(node, timezone) + + if args.test_time: + t = get_time() + print(f'{t.hour:02d}:{t.minute:02d}') + return + + with open(schedule_file, 'r') as f: + schedule_yaml = yaml.safe_load(f) + + unsorted_schedule = {} + for fleet, change in schedule_yaml.items(): + for time_text, assignments in change.items(): + time = ScheduleTimePoint.parse(time_text) + entry: list[Assignment] = unsorted_schedule.get(time, list()) + for robot, charger in assignments.items(): + entry.append(Assignment(fleet, robot, charger)) + unsorted_schedule[time] = entry + + schedule = {} + sorted_times = [] + for time in sorted(unsorted_schedule.keys()): + sorted_times.append(time) + schedule[time] = unsorted_schedule[time] + + num_fleets = len(schedule_yaml.keys()) + transient_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=2*num_fleets, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + publisher = node.create_publisher( + ChargingAssignments, 'charging_assignments', transient_qos + ) + + # fleet -> robot -> charger + assignments = {} + last_update_index = bisect.bisect_right(sorted_times, get_time()) + update_assignments( + None, last_update_index, + sorted_times, schedule, assignments, publisher, node, + ) + + def update(): + nonlocal last_update_index + nonlocal sorted_times + nonlocal schedule + nonlocal assignments + nonlocal publisher + + next_update_index = bisect.bisect_right(sorted_times, get_time()) + if last_update_index < next_update_index: + update_assignments( + last_update_index, next_update_index, + sorted_times, schedule, assignments, publisher, node, + ) + last_update_index = next_update_index + + elif next_update_index < last_update_index: + # The cycle must have restarted, e.g. passing midnight + update_assignments( + None, next_update_index, + sorted_times, schedule, assignments, publisher, node, + ) + last_update_index = next_update_index + + node.create_timer(10.0, update) + + rclpy.spin(node) + +if __name__ == '__main__': + main(sys.argv) diff --git a/rmf_charging_schedule/setup.cfg b/rmf_charging_schedule/setup.cfg new file mode 100644 index 000000000..fb5d24c84 --- /dev/null +++ b/rmf_charging_schedule/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_charging_schedule +[install] +install_scripts=$base/lib/rmf_charging_schedule diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py new file mode 100644 index 000000000..6ae87a05e --- /dev/null +++ b/rmf_charging_schedule/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup, find_packages + +package_name = 'rmf_charging_schedule' + +setup( + name=package_name, + version='2.3.2', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Grey', + maintainer_email='mxgrey@intrinsic.ai', + description='A node that manages a fixed schedule for robot charger usage', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'charging_schedule=rmf_charging_schedule.main:main', + ], + }, +) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index c38326902..152250741 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -255,6 +255,25 @@ target_include_directories(lift_supervisor # ----------------------------------------------------------------------------- +add_executable(mutex_group_supervisor + src/mutex_group_supervisor/main.cpp +) + +target_link_libraries(mutex_group_supervisor + PRIVATE + rmf_fleet_adapter + ${rclcpp_LIBARRIES} + ${rmf_fleet_msgs_LIBRARIES} +) + +target_include_directories(mutex_group_supervisor + PRIVATE + ${rclcpp_INCLUDE_DIRS} + ${rmf_fleet_msgs_INCLUDE_DIRS} +) + +# ----------------------------------------------------------------------------- + add_executable(experimental_lift_watchdog src/experimental_lift_watchdog/main.cpp ) @@ -502,6 +521,7 @@ install( mock_traffic_light full_control lift_supervisor + mutex_group_supervisor experimental_lift_watchdog door_supervisor robot_state_aggregator diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 9d0ac0591..a736ce9e5 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -68,6 +68,13 @@ const std::string InterruptRequestTopicName = "robot_interrupt_request"; const std::string TaskApiRequests = "task_api_requests"; const std::string TaskApiResponses = "task_api_responses"; +const std::string ChargingAssignmentsTopicName = "charging_assignments"; + +const std::string MutexGroupRequestTopicName = "mutex_group_request"; +const std::string MutexGroupStatesTopicName = "mutex_group_states"; + +const uint64_t Unclaimed = (uint64_t)(-1); + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index b5f9531c9..2be6c0698 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -74,7 +74,11 @@ class EasyFullControl : public std::enable_shared_from_this class CommandExecution; /// Signature for a function that handles navigation requests. The request - /// will specify an (x, y) location and yaw on a map. + /// will specify a destination for the robot to go to. + /// + /// \param[in] destination + /// Where the robot should move to. Includings (x, y) coordinates, a target + /// yaw, a map name, and may include a graph index when one is available. /// /// \param[in] execution /// The command execution progress updater. Use this to keep the fleet @@ -87,6 +91,19 @@ class EasyFullControl : public std::enable_shared_from_this /// Signature for a function to handle stop requests. using StopRequest = std::function; + /// Signature for a function that handles localization requests. The request + /// will specify an approximate location for the robot. + /// + /// \param[in] location_estimate + /// An estimate for where the robot is currently located. + /// + /// \param[in] execution + /// The command execution progress updater. Use this to keep the fleet + /// adapter updated on the progress of localizing. + using LocalizationRequest = std::function; + /// Add a robot to the fleet once it is available. /// /// \param[in] name @@ -333,6 +350,13 @@ class EasyFullControl::RobotCallbacks /// Get the action executor. ActionExecutor action_executor() const; + /// Give the robot a localization callback. Unlike the callbacks used by the + /// constructor, this callback is optional. + RobotCallbacks& with_localization(LocalizationRequest localization); + + /// Get the callback for localizing if available. + LocalizationRequest localize() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; @@ -414,6 +438,10 @@ class EasyFullControl::Destination /// from this field. std::optional graph_index() const; + /// The name of this destination, if it has one. Nameless destinations will + /// give an empty string. + std::string name() const; + /// If there is a speed limit that should be respected while approaching the /// destination, this will indicate it. std::optional speed_limit() const; @@ -422,6 +450,10 @@ class EasyFullControl::Destination /// will contain the name of the dock. std::optional dock() const; + /// Get whether the destination is inside of a lift, and if so get the + /// properties of the lift. + rmf_traffic::agv::Graph::LiftPropertiesPtr inside_lift() const; + class Implementation; private: Destination(); @@ -743,6 +775,38 @@ class EasyFullControl::FleetConfiguration /// Set the minimum lane length. void set_default_min_lane_length(double distance); + /// During a fire emergency, real-life lifts might be required to move to a + /// specific level and refuse to stop or go to any other level. This function + /// lets you provide this information to the fleet adapter so that it can + /// produce reasonable emergency pullover plans for robots that happen to be + /// inside of a lift when the fire alarm goes off. + /// + /// Internally, this will close all lanes that go into the specified lift and + /// close all lanes exiting this lift (except on the designated level) when a + /// fire emergency begins. Lifts that were not specified in a call to this + /// function will not behave any differently during a fire emergency. + /// + /// \param[in] lift_name + /// The name of the lift whose behavior is being specified + /// + /// \param[in] emergency_level_name + /// The level that lift will go to when a fire emergency is happening + void set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name); + + /// Get mutable access to the level that each specified lift will go to during + /// a fire emergency. + /// + /// \sa set_lift_emergency_level + std::unordered_map& change_lift_emergency_levels(); + + /// Get the level that each specified lift will go to during a fire emergency. + /// + /// \sa set_lift_emergency_level + const std::unordered_map& + lift_emergency_levels() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 4bd5ad23d..290f5bd35 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -210,6 +210,26 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// Specify a set of lanes that should be open. void open_lanes(std::vector lane_indices); + /// During a fire emergency, real-life lifts might be required to move to a + /// specific level and refuse to stop or go to any other level. This function + /// lets you provide this information to the fleet adapter so that it can + /// produce reasonable emergency pullover plans for robots that happen to be + /// inside of a lift when the fire alarm goes off. + /// + /// Internally, this will close all lanes that go into the specified lift and + /// close all lanes exiting this lift (except on the designated level) when a + /// fire emergency begins. Lifts that were not specified in a call to this + /// function will not behave any differently during a fire emergency. + /// + /// \param[in] lift_name + /// The name of the lift whose behavior is being specified + /// + /// \param[in] emergency_level_name + /// The level that lift will go to when a fire emergency is happening + void set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name); + /// A class used to describe speed limit imposed on lanes. class SpeedLimitRequest { diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index d0aa2c22e..22da7b444 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -126,6 +126,10 @@ class RobotUpdateHandle /// value that was given to the setter. rmf_utils::optional maximum_delay() const; + /// Get the current task ID of the robot, or an empty string if the robot + /// is not performing any task. + const std::string current_task_id() const; + /// Unique identifier for an activity that the robot is performing. Used by /// the EasyFullControl API. class ActivityIdentifier @@ -404,6 +408,9 @@ class RobotUpdateHandle /// By default this behavior is enabled. void enable_responsive_wait(bool value); + /// If the robot is holding onto a session with a lift, release that session. + void release_lift(); + class Implementation; /// This API is experimental and will not be supported in the future. Users @@ -471,6 +478,9 @@ class RobotUpdateHandle Watchdog watchdog, rmf_traffic::Duration wait_duration = std::chrono::seconds(10)); + /// Turn on/off a debug dump of how position updates are being processed + void debug_positions(bool on); + private: friend Implementation; Implementation* _pimpl; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp new file mode 100644 index 000000000..01a2d315b --- /dev/null +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2023 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 RMF_FLEET_ADAPTER__TASKS__PARKROBOT_HPP +#define RMF_FLEET_ADAPTER__TASKS__PARKROBOT_HPP + +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +/// Use this task factory to make finisher tasks (idle tasks) that will move the +/// robot to a parking spot. +class ParkRobotIndefinitely : public rmf_task::RequestFactory +{ +public: + /// Constructor + /// + /// \param[in] requester + /// The identifier of the entity that owns this RequestFactory, that will be + /// the designated requester of each new request. + /// + /// \param[in] time_now_cb + /// Callback function that returns the current time. + /// + /// \param[in] parking_waypoint + /// The graph index of the waypoint assigned to this AGV for parking. + /// If nullopt, the AGV will return to its charging_waypoint and remain idle + /// there. It will not wait for its battery to charge up before undertaking + /// new tasks. + ParkRobotIndefinitely( + const std::string& requester, + std::function time_now_cb, + std::optional parking_waypoint = std::nullopt); + + rmf_task::ConstRequestPtr make_request( + const rmf_task::State& state) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // RMF_FLEET_ADAPTER__AGV__PARKROBOT_HPP diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index e979212fd..2888482da 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -1249,10 +1249,12 @@ std::shared_ptr make_fleet( if (finishing_request_string == "charge") { - finishing_request = + auto charge_factory = std::make_shared( std::string(node->get_name()), std::move(get_time)); + charge_factory->set_indefinite(true); + finishing_request = charge_factory; RCLCPP_INFO( node->get_logger(), "Fleet is configured to perform ChargeBattery as finishing request"); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index fc096dcd0..77c05fdcb 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -28,12 +28,14 @@ Node::Node() : rclcpp::Node("rmf_lift_supervisor") { const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto transient_qos = rclcpp::SystemDefaultsQoS() + .reliable().keep_last(100).transient_local(); _lift_request_pub = create_publisher( - FinalLiftRequestTopicName, default_qos); + FinalLiftRequestTopicName, transient_qos); _adapter_lift_request_sub = create_subscription( - AdapterLiftRequestTopicName, default_qos, + AdapterLiftRequestTopicName, transient_qos, [&](LiftRequest::UniquePtr msg) { _adapter_lift_request_update(std::move(msg)); @@ -53,6 +55,12 @@ Node::Node() //============================================================================== void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) { + + RCLCPP_INFO( + this->get_logger(), + "[%s] Received adapter lift request to [%s] with request type [%d]", + msg->session_id.c_str(), msg->destination_floor.c_str(), msg->request_type + ); auto& curr_request = _active_sessions.insert( std::make_pair(msg->lift_name, nullptr)).first->second; @@ -64,7 +72,13 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) curr_request = std::move(msg); else { + msg->request_time = this->now(); _lift_request_pub->publish(*msg); + RCLCPP_INFO( + this->get_logger(), + "[%s] Published end lift session from lift supervisor", + msg->session_id.c_str() + ); curr_request = nullptr; } } @@ -90,7 +104,25 @@ void Node::_lift_state_update(LiftState::UniquePtr msg) { if ((lift_request->destination_floor != msg->current_floor) || (lift_request->door_state != msg->door_state)) - _lift_request_pub->publish(*lift_request); + lift_request->request_time = this->now(); + _lift_request_pub->publish(*lift_request); + RCLCPP_INFO( + this->get_logger(), + "[%s] Published lift request to [%s] from lift supervisor", + msg->session_id.c_str(), lift_request->destination_floor.c_str() + ); + } + else + { + // If there are no active sessions going on, we keep publishing session + // end requests to ensure that the lift is released + LiftRequest request; + request.lift_name = msg->lift_name; + request.destination_floor = msg->current_floor; + request.session_id = msg->session_id; + request.request_time = this->now(); + request.request_type = LiftRequest::REQUEST_END_SESSION; + _lift_request_pub->publish(request); } // For now, we do not need to publish this. diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp new file mode 100644 index 000000000..4f07df44d --- /dev/null +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2023 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 +#include +#include + +#include +#include + +#include +#include +#include + +struct TimeStamps +{ + rclcpp::Time claim_time; + std::chrono::steady_clock::time_point heartbeat_time; +}; + +using ClaimMap = std::unordered_map; +using MutexGroupRequest = rmf_fleet_msgs::msg::MutexGroupRequest; +using MutexGroupStates = rmf_fleet_msgs::msg::MutexGroupStates; +using MutextGroupAssignment = rmf_fleet_msgs::msg::MutexGroupAssignment; + +const uint64_t Unclaimed = rmf_fleet_adapter::Unclaimed; + +class Node : public rclcpp::Node +{ +public: + Node() + : rclcpp::Node("mutex_group_supervisor") + { + const auto qos = rclcpp::SystemDefaultsQoS() + .reliable() + .transient_local() + .keep_last(100); + + latest_states = rmf_fleet_msgs::build() + .assignments({}); + + request_sub = create_subscription( + rmf_fleet_adapter::MutexGroupRequestTopicName, + qos, + [&](const MutexGroupRequest& request) + { + handle_request(request); + }); + + state_pub = create_publisher( + rmf_fleet_adapter::MutexGroupStatesTopicName, + qos); + + heartbeat_timer = create_wall_timer( + std::chrono::seconds(2), + [&]() { do_heartbeat(); }); + } + + void handle_request(const MutexGroupRequest& request) + { + auto& claims = mutex_groups[request.group]; + if (request.mode == request.MODE_RELEASE) + { + const auto g_it = mutex_groups.find(request.group); + if (g_it != mutex_groups.end()) + { + const auto c_it = g_it->second.find(request.claimant); + if (c_it != g_it->second.end()) + { + if (c_it->second.claim_time <= request.claim_time) + { + g_it->second.erase(c_it); + pick_next(request.group); + state_pub->publish(latest_states); + } + } + } + return; + } + + auto now = std::chrono::steady_clock::now(); + auto claim_time = rclcpp::Time(request.claim_time); + auto timestamps = TimeStamps { request.claim_time, now }; + claims.insert_or_assign(request.claimant, timestamps); + for (const auto& s : latest_states.assignments) + { + if (s.group == request.group && s.claimant != Unclaimed) + { + check_for_conflicts(); + return; + } + } + pick_next(request.group); + } + + void do_heartbeat() + { + const auto now = std::chrono::steady_clock::now(); + // TODO(MXG): Make this timeout configurable + const auto timeout = std::chrono::seconds(10); + for (auto& [group, claims] : mutex_groups) + { + std::vector remove_claims; + for (const auto& [claimant, timestamp] : claims) + { + if (timestamp.heartbeat_time + timeout < now) + { + remove_claims.push_back(claimant); + } + } + + uint64_t current_claimer = Unclaimed; + for (const auto& assignment : latest_states.assignments) + { + if (assignment.group == group) + { + current_claimer = assignment.claimant; + break; + } + } + + bool need_next_pick = false; + for (const auto& remove_claim : remove_claims) + { + if (current_claimer == remove_claim) + { + need_next_pick = true; + } + + claims.erase(remove_claim); + } + + if (need_next_pick) + { + pick_next(group); + } + } + + state_pub->publish(latest_states); + } + + struct ClaimList + { + std::unordered_set groups; + std::optional earliest_time; + + void insert(std::string group, rclcpp::Time time) + { + groups.insert(std::move(group)); + if (!earliest_time.has_value() || time < *earliest_time) + { + earliest_time = time; + } + } + + void normalize( + uint64_t claimant, + std::unordered_map& mutex_groups) const + { + if (!earliest_time.has_value()) + return; + + for (const auto& group : groups) + { + mutex_groups[group][claimant].claim_time = *earliest_time; + } + } + }; + + void check_for_conflicts() + { + std::unordered_map claims; + for (const auto& [group, group_claims] : mutex_groups) + { + for (const auto& [claimant, timestamps] : group_claims) + { + claims[claimant].insert(group, timestamps.claim_time); + } + } + + std::unordered_set normalized_groups; + for (auto i = claims.begin(); i != claims.end(); ++i) + { + auto j = i; + ++j; + for (; j != claims.end(); ++j) + { + const auto& claim_i = i->second; + const auto& claim_j = j->second; + if (claim_i.groups == claim_j.groups && claim_i.groups.size() > 1) + { + claim_i.normalize(i->first, mutex_groups); + claim_j.normalize(j->first, mutex_groups); + + std::stringstream ss; + for (const auto& group : claim_i.groups) + { + normalized_groups.insert(group); + ss << "[" << group << "]"; + } + + RCLCPP_INFO( + get_logger(), + "Resolving mutex conflict between claimants [%lu] and [%lu] which both " + "want the mutex combination %s", + i->first, + j->first, + ss.str().c_str()); + } + } + } + + for (const auto& group : normalized_groups) + pick_next(group); + } + + void pick_next(const std::string& group) + { + const auto& claimants = mutex_groups[group]; + std::optional> earliest; + for (const auto& [claimant, timestamp] : claimants) + { + const auto& t = timestamp.claim_time; + if (!earliest.has_value() || t < earliest->first) + { + earliest = std::make_pair(t, claimant); + } + } + + uint64_t claimant = Unclaimed; + builtin_interfaces::msg::Time claim_time; + if (earliest.has_value()) + { + claim_time = earliest->first; + claimant = earliest->second; + } + bool group_found = false; + for (auto& a : latest_states.assignments) + { + if (a.group == group) + { + a.claimant = claimant; + a.claim_time = claim_time; + group_found = true; + break; + } + } + if (!group_found) + { + latest_states.assignments.push_back( + rmf_fleet_msgs::build() + .group(group) + .claimant(claimant) + .claim_time(claim_time)); + } + } + + std::unordered_map mutex_groups; + rclcpp::Subscription::SharedPtr request_sub; + rclcpp::Publisher::SharedPtr state_pub; + rclcpp::TimerBase::SharedPtr heartbeat_timer; + MutexGroupStates latest_states; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp index c580924da..9c205e34c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp @@ -34,6 +34,8 @@ namespace rmf_fleet_adapter { +using PlanIdPtr = std::shared_ptr; + //============================================================================== class LegacyTask : public std::enable_shared_from_this { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index cbda9b141..67d9cc15e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -73,6 +73,10 @@ TaskManagerPtr TaskManager::make( std::optional> broadcast_client, std::weak_ptr fleet_handle) { + std::shared_ptr fleet = fleet_handle.lock(); + if (!fleet) + return nullptr; + auto mgr = TaskManagerPtr( new TaskManager( std::move(context), @@ -96,6 +100,7 @@ TaskManagerPtr TaskManager::make( auto task_id = "emergency_pullover." + self->_context->name() + "." + self->_context->group() + "-" + std::to_string(self->_count_emergency_pullover++); + self->_context->current_task_id(task_id); // TODO(MXG): Consider subscribing to the emergency pullover update self->_emergency_pullover = ActiveTask::start( @@ -115,18 +120,19 @@ TaskManagerPtr TaskManager::make( }); }; - mgr->_emergency_sub = mgr->_context->node()->emergency_notice() + mgr->_emergency_sub = agv::FleetUpdateHandle::Implementation::get(*fleet) + .emergency_obs .observe_on(rxcpp::identity_same_worker(mgr->_context->worker())) .subscribe( - [w = mgr->weak_from_this(), begin_pullover](const auto& msg) + [w = mgr->weak_from_this(), begin_pullover](const bool is_emergency) { if (auto mgr = w.lock()) { - if (mgr->_emergency_active == msg->data) + if (mgr->_emergency_active == is_emergency) return; - mgr->_emergency_active = msg->data; - if (msg->data) + mgr->_emergency_active = is_emergency; + if (is_emergency) { if (mgr->_waiting) { @@ -905,6 +911,20 @@ void TaskManager::enable_responsive_wait(bool value) } } +//============================================================================== +void TaskManager::set_idle_task(rmf_task::ConstRequestFactoryPtr task) +{ + if (_idle_task == task) + return; + + _idle_task = std::move(task); + std::lock_guard guard(_mutex); + if (!_active_task && _queue.empty() && _direct_queue.empty()) + { + _begin_waiting(); + } +} + //============================================================================== void TaskManager::set_queue( const std::vector& assignments) @@ -1268,7 +1288,7 @@ void TaskManager::_begin_next_task() if (_queue.empty() && _direct_queue.empty()) { - if (!_waiting) + if (!_waiting && !_finished_waiting) _begin_waiting(); return; @@ -1309,6 +1329,7 @@ void TaskManager::_begin_next_task() id.c_str(), _context->requester_id().c_str()); + _finished_waiting = false; _context->current_task_end_state(assignment.finish_state()); _context->current_task_id(id); _active_task = ActiveTask::start( @@ -1360,7 +1381,7 @@ void TaskManager::_begin_next_task() } else { - if (!_waiting) + if (!_waiting && !_finished_waiting) _begin_waiting(); } @@ -1457,6 +1478,23 @@ std::function TaskManager::_robot_interruption_callback() //============================================================================== void TaskManager::_begin_waiting() { + if (_idle_task) + { + const auto request = _idle_task->make_request(_context->make_get_state()()); + _waiting = ActiveTask::start( + _context->task_activator()->activate( + _context->make_get_state(), + _context->task_parameters(), + *request, + _update_cb(), + _checkpoint_cb(), + _phase_finished_cb(), + _make_resume_from_waiting()), + _context->now()); + _context->current_task_id(request->booking()->id()); + return; + } + if (!_responsive_wait_enabled) return; @@ -1500,6 +1538,7 @@ void TaskManager::_begin_waiting() _update_cb(), _make_resume_from_waiting()), _context->now()); + _context->current_task_id(task_id); } //============================================================================== @@ -1564,6 +1603,7 @@ std::function TaskManager::_make_resume_from_waiting() if (!self) return; + self->_finished_waiting = true; self->_waiting = ActiveTask(); self->_begin_next_task(); }); @@ -2151,6 +2191,7 @@ std::function TaskManager::_task_finished(std::string id) // Publish the final state of the task before destructing it self->_publish_task_state(); self->_active_task = ActiveTask(); + self->_context->current_task_id(std::nullopt); self->_context->worker().schedule( [w = self->weak_from_this()](const auto&) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 1d8759bcc..52de08f81 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -111,6 +111,8 @@ class TaskManager : public std::enable_shared_from_this void enable_responsive_wait(bool value); + void set_idle_task(rmf_task::ConstRequestFactoryPtr task); + /// Set the queue for this task manager with assignments generated from the /// task planner void set_queue(const std::vector& assignments); @@ -308,6 +310,9 @@ class TaskManager : public std::enable_shared_from_this std::weak_ptr _fleet_handle; rmf_task::ConstActivatorPtr _task_activator; ActiveTask _active_task; + /// The task that should be performed when the robot is idle. This takes + /// precedent over responsive waiting. + rmf_task::ConstRequestFactoryPtr _idle_task; bool _responsive_wait_enabled = false; bool _emergency_active = false; std::optional _emergency_pullover_interrupt_token; @@ -327,6 +332,7 @@ class TaskManager : public std::enable_shared_from_this /// will ensure that the agent continues to respond to traffic negotiations so /// it does not become a blocker for other traffic participants. ActiveTask _waiting; + bool _finished_waiting = false; uint16_t _count_waiting = 0; // TODO: Eliminate the need for a mutex by redesigning the use of the task diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index de6eb68e1..2bb577397 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -264,7 +264,7 @@ std::shared_ptr Adapter::add_easy_fleet( if (entry) entry->execute(finder); - const auto* exit = config.graph()->get_lane(i).entry().event(); + const auto* exit = config.graph()->get_lane(i).exit().event(); if (exit) exit->execute(finder); } @@ -367,6 +367,11 @@ std::shared_ptr Adapter::add_easy_fleet( *config.transformations_to_robot_coordinates()); } + for (const auto& [lift, level] : config.lift_emergency_levels()) + { + fleet_handle->set_lift_emergency_level(lift, level); + } + return EasyFullControl::Implementation::make( fleet_handle, config.skip_rotation_commands(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index a60f57043..bc9db015c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -217,6 +218,7 @@ class EasyFullControl::RobotCallbacks::Implementation NavigationRequest navigate; StopRequest stop; ActionExecutor action_executor; + LocalizationRequest localize; }; //============================================================================== @@ -227,7 +229,8 @@ EasyFullControl::RobotCallbacks::RobotCallbacks( : _pimpl(rmf_utils::make_impl(Implementation{ std::move(navigate), std::move(stop), - std::move(action_executor) + std::move(action_executor), + nullptr })) { // Do nothing @@ -252,334 +255,409 @@ auto EasyFullControl::RobotCallbacks::action_executor() const -> ActionExecutor } //============================================================================== -class EasyFullControl::CommandExecution::Implementation +auto EasyFullControl::RobotCallbacks::with_localization( + LocalizationRequest localization) -> RobotCallbacks& +{ + _pimpl->localize = std::move(localization); + return *this; +} + +//============================================================================== +auto EasyFullControl::RobotCallbacks::localize() const -> LocalizationRequest +{ + return _pimpl->localize; +} + +//============================================================================== +class EasyFullControl::CommandExecution::Implementation::Data { public: - struct Data + std::vector waypoints; + std::vector lanes; + std::optional target_location; + std::optional final_orientation; + rmf_traffic::Duration planned_wait_time; + std::optional schedule_override; + std::shared_ptr nav_params; + std::function arrival_estimator; + + void release_stubbornness() { - std::vector waypoints; - std::vector lanes; - std::optional final_orientation; - rmf_traffic::Duration planned_wait_time; - std::optional schedule_override; - std::shared_ptr nav_params; - std::function arrival_estimator; + if (schedule_override.has_value()) + { + schedule_override->release_stubbornness(); + } + } - void release_stubbornness() + void update_location( + const std::shared_ptr& context, + const std::string& map, + Eigen::Vector3d location) + { + if (schedule_override.has_value()) { - if (schedule_override.has_value()) - { - schedule_override->release_stubbornness(); - } + return schedule_override->overridden_update( + context, + map, + location); } - void update_location( - const std::shared_ptr& context, - const std::string& map, - Eigen::Vector3d location) + auto planner = context->planner(); + if (!planner) { - if (schedule_override.has_value()) - { - return schedule_override->overridden_update( - context, - map, - location); - } + RCLCPP_ERROR( + context->node()->get_logger(), + "Planner unavailable for robot [%s], cannot update its location", + context->requester_id().c_str()); + return; + } - auto planner = context->planner(); - if (!planner) + const auto& graph = planner->get_configuration().graph(); + const rmf_traffic::agv::LaneClosure* closures = + context->get_lane_closures(); + std::optional> on_waypoint; + auto p = Eigen::Vector2d(location[0], location[1]); + const double yaw = location[2]; + for (std::size_t wp : waypoints) + { + if (wp >= graph.num_waypoints()) { RCLCPP_ERROR( context->node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context->requester_id().c_str()); + "Robot [%s] has a command with a waypoint [%lu] that is outside " + "the range of the graph [%lu]. We will not do a location update.", + context->requester_id().c_str(), + wp, + graph.num_waypoints()); + // Should we also issue a replan command? return; } - const auto& graph = planner->get_configuration().graph(); - const auto& closed_lanes = planner->get_configuration().lane_closures(); - std::optional> on_waypoint; - auto p = Eigen::Vector2d(location[0], location[1]); - const double yaw = location[2]; - for (std::size_t wp : waypoints) + const auto p_wp = graph.get_waypoint(wp).get_location(); + auto dist = (p - p_wp).norm(); + if (dist <= nav_params->max_merge_waypoint_distance) + { + if (!on_waypoint.has_value() || dist < on_waypoint->second) + { + on_waypoint = std::make_pair(wp, dist); + } + } + } + + rmf_traffic::agv::Plan::StartSet starts; + const auto now = rmf_traffic_ros2::convert(context->node()->now()); + if (on_waypoint.has_value()) + { + const auto wp = on_waypoint->first; + starts.push_back(rmf_traffic::agv::Plan::Start(now, wp, yaw, p)); + for (std::size_t lane_id : graph.lanes_from(wp)) { - if (wp >= graph.num_waypoints()) + if (lane_id >= graph.num_lanes()) { RCLCPP_ERROR( context->node()->get_logger(), - "Robot [%s] has a command with a waypoint [%lu] that is outside " - "the range of the graph [%lu]. We will not do a location update.", + "Nav graph for robot [%s] has an invalid lane ID [%lu] leaving " + "vertex [%lu], lane ID range is [%lu]. We will not do a location " + "update.", context->requester_id().c_str(), + lane_id, wp, - graph.num_waypoints()); + graph.num_lanes()); // Should we also issue a replan command? return; } - const auto p_wp = graph.get_waypoint(wp).get_location(); - auto dist = (p - p_wp).norm(); - if (dist <= nav_params->max_merge_waypoint_distance) + if (closures && closures->is_closed(lane_id)) { - if (!on_waypoint.has_value() || dist < on_waypoint->second) - { - on_waypoint = std::make_pair(wp, dist); - } + // Don't use a lane that's closed + continue; } - } - rmf_traffic::agv::Plan::StartSet starts; - const auto now = rmf_traffic_ros2::convert(context->node()->now()); - if (on_waypoint.has_value()) + auto wp_exit = graph.get_lane(lane_id).exit().waypoint_index(); + starts.push_back( + rmf_traffic::agv::Plan::Start(now, wp_exit, yaw, p, lane_id)); + } + } + else + { + std::optional> on_lane; + for (auto lane_id : lanes) { - const auto wp = on_waypoint->first; - starts.push_back(rmf_traffic::agv::Plan::Start(now, wp, yaw, p)); - for (std::size_t lane_id : graph.lanes_from(wp)) + if (lane_id >= graph.num_lanes()) { - if (lane_id >= graph.num_lanes()) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "Nav graph for robot [%s] has an invalid lane ID [%lu] leaving " - "vertex [%lu], lane ID range is [%lu]. We will not do a location " - "update.", - context->requester_id().c_str(), - lane_id, - wp, - graph.num_lanes()); - // Should we also issue a replan command? - return; - } + RCLCPP_ERROR( + context->node()->get_logger(), + "Robot [%s] has a command with a lane [%lu] that is outside the " + "range of the graph [%lu]. We will not do a location update.", + context->requester_id().c_str(), + lane_id, + graph.num_lanes()); + // Should we also issue a replan command? + return; + } - if (closed_lanes.is_closed(lane_id)) - { - // Don't use a lane that's closed - continue; - } + if (closures && closures->is_closed(lane_id)) + { + continue; + } - auto wp_exit = graph.get_lane(lane_id).exit().waypoint_index(); - starts.push_back( - rmf_traffic::agv::Plan::Start(now, wp_exit, yaw, p, lane_id)); + const auto& lane = graph.get_lane(lane_id); + const auto p0 = + graph.get_waypoint(lane.entry().waypoint_index()).get_location(); + const auto p1 = + graph.get_waypoint(lane.exit().waypoint_index()).get_location(); + const auto lane_length = (p1 - p0).norm(); + double dist_to_lane = 0.0; + if (lane_length < nav_params->min_lane_length) + { + dist_to_lane = std::min( + (p - p0).norm(), + (p - p1).norm()); } - } - else - { - std::optional> on_lane; - for (auto lane_id : lanes) + else { - if (lane_id >= graph.num_lanes()) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "Robot [%s] has a command with a lane [%lu] that is outside the " - "range of the graph [%lu]. We will not do a location update.", - context->requester_id().c_str(), - lane_id, - graph.num_lanes()); - // Should we also issue a replan command? - return; - } - - if (closed_lanes.is_closed(lane_id)) - { - continue; - } - - const auto& lane = graph.get_lane(lane_id); - const auto p0 = - graph.get_waypoint(lane.entry().waypoint_index()).get_location(); - const auto p1 = - graph.get_waypoint(lane.exit().waypoint_index()).get_location(); - const auto lane_length = (p1 - p0).norm(); const auto lane_u = (p1 - p0)/lane_length; const auto proj = (p - p0).dot(lane_u); if (proj < 0.0 || lane_length < proj) { continue; } - - const auto dist_to_lane = (p - p0 - proj * lane_u).norm(); - if (dist_to_lane <= nav_params->max_merge_lane_distance) - { - if (!on_lane.has_value() || dist_to_lane < on_lane->second) - { - on_lane = std::make_pair(lane_id, dist_to_lane); - } - } + dist_to_lane = (p - p0 - proj * lane_u).norm(); } - if (on_lane.has_value()) + if (dist_to_lane <= nav_params->max_merge_lane_distance) { - const auto lane_id = on_lane->first; - const auto& lane = graph.get_lane(lane_id); - const auto wp0 = lane.entry().waypoint_index(); - const auto wp1 = lane.exit().waypoint_index(); - starts.push_back( - rmf_traffic::agv::Plan::Start(now, wp1, yaw, p, lane_id)); - - if (const auto* reverse_lane = graph.lane_from(wp1, wp0)) + if (!on_lane.has_value() || dist_to_lane < on_lane->second) { - starts.push_back(rmf_traffic::agv::Plan::Start( - now, wp0, yaw, p, reverse_lane->index())); + on_lane = std::make_pair(lane_id, dist_to_lane); } } - else - { - starts = nav_params->compute_plan_starts(graph, map, location, now); - } } - if (!starts.empty()) + if (on_lane.has_value()) { - context->set_location(starts); + const auto lane_id = on_lane->first; + const auto& lane = graph.get_lane(lane_id); + const auto wp0 = lane.entry().waypoint_index(); + const auto wp1 = lane.exit().waypoint_index(); + starts.push_back( + rmf_traffic::agv::Plan::Start(now, wp1, yaw, p, lane_id)); + + if (const auto* reverse_lane = graph.lane_from(wp1, wp0)) + { + starts.push_back(rmf_traffic::agv::Plan::Start( + now, wp0, yaw, p, reverse_lane->index())); + } } - else + } + + if (starts.empty()) + { + starts = nav_params->compute_plan_starts(graph, map, location, now); + } + + if (!starts.empty()) + { + if (context->debug_positions) { - context->set_lost(Location { now, map, location }); + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, graph); + std::cout << ss.str() << std::endl; } - - if (!waypoints.empty()) + context->set_location(starts); + } + else + { + if (context->debug_positions) { - const auto p_final = - graph.get_waypoint(waypoints.back()).get_location(); - const auto distance = (p_final - p).norm(); - double rotation = 0.0; - if (final_orientation.has_value()) + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map << " <" << location.block<2, 1>(0, 0).transpose() + << "> orientation " << location[2] * 180.0 / M_PI << "\n"; + ss << waypoints.size() << " waypoints:"; + for (std::size_t wp : waypoints) { - rotation = - std::fabs(rmf_utils::wrap_to_pi(location[2] - *final_orientation)); - const auto reversible = - planner->get_configuration().vehicle_traits() - .get_differential()->is_reversible(); - if (reversible) - { - const double alternate_orientation = rmf_utils::wrap_to_pi( - *final_orientation + M_PI); - - const double alternate_rotation = std::fabs( - rmf_utils::wrap_to_pi(location[2] - alternate_orientation)); - rotation = std::min(rotation, alternate_rotation); - } + ss << "\n -- " << print_waypoint(wp, graph); + } + ss << lanes.size() << " lanes:"; + for (std::size_t lane : lanes) + { + ss << "\n -- " << print_lane(lane, graph); } - const auto& traits = planner->get_configuration().vehicle_traits(); - const auto v = std::max(traits.linear().get_nominal_velocity(), 0.001); - const auto w = - std::max(traits.rotational().get_nominal_velocity(), 0.001); - const auto t = distance / v + rotation / w; - arrival_estimator( - rmf_traffic::time::from_seconds(t) + planned_wait_time); + std::cout << ss.str() << std::endl; } + context->set_lost(Location { now, map, location }); } - }; - using DataPtr = std::shared_ptr; - - std::weak_ptr w_context; - std::shared_ptr data; - std::function begin; - std::function finisher; - ActivityIdentifierPtr identifier; - void finish() - { - if (auto context = w_context.lock()) + if (target_location.has_value()) { - context->worker().schedule( - [ - context = context, - data = this->data, - identifier = this->identifier, - finisher = this->finisher - ](const auto&) + const auto p_final = *target_location; + const auto distance = (p_final - p).norm(); + double rotation = 0.0; + if (final_orientation.has_value()) + { + rotation = + std::fabs(rmf_utils::wrap_to_pi(location[2] - *final_orientation)); + const auto reversible = + planner->get_configuration().vehicle_traits() + .get_differential()->is_reversible(); + if (reversible) { - if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) - { - // This activity has already finished - return; - } + const double alternate_orientation = rmf_utils::wrap_to_pi( + *final_orientation + M_PI); - // Prevent this activity from doing any further updates - ActivityIdentifier::Implementation::get( - *identifier).update_fn = nullptr; - if (data->schedule_override.has_value()) - { - data->release_stubbornness(); - RCLCPP_INFO( - context->node()->get_logger(), - "Requesting replan for [%s] after finishing a schedule override", - context->requester_id().c_str()); - context->request_replan(); - } - else - { - // Trigger the next step in the sequence - finisher(); - } - }); + const double alternate_rotation = std::fabs( + rmf_utils::wrap_to_pi(location[2] - alternate_orientation)); + rotation = std::min(rotation, alternate_rotation); + } + } + + const auto& traits = planner->get_configuration().vehicle_traits(); + const auto v = std::max(traits.linear().get_nominal_velocity(), 0.001); + const auto w = + std::max(traits.rotational().get_nominal_velocity(), 0.001); + const auto t = distance / v + rotation / w; + arrival_estimator( + rmf_traffic::time::from_seconds(t) + planned_wait_time); } } +}; - Stubbornness override_schedule( - std::string map, - std::vector path, - rmf_traffic::Duration hold) +void EasyFullControl::CommandExecution::Implementation::finish() +{ + if (auto context = w_context.lock()) { - auto stubborn = std::make_shared(); - if (const auto context = w_context.lock()) - { - context->worker().schedule( - [ - context, - stubborn, - data = this->data, - identifier = this->identifier, - map = std::move(map), - path = std::move(path), - hold - ](const auto&) + context->worker().schedule( + [ + context = context, + data = this->data, + identifier = this->identifier, + finisher = this->finisher + ](const auto&) + { + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) { - if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) - { - // Don't do anything because this command is finished - return; - } + // This activity has already finished + return; + } + // Prevent this activity from doing any further updates + ActivityIdentifier::Implementation::get( + *identifier).update_fn = nullptr; + if (data && data->schedule_override.has_value()) + { data->release_stubbornness(); - data->schedule_override = ScheduleOverride::make( - context, map, path, hold, stubborn); - }); - } - - return Stubbornness::Implementation::make(stubborn); + RCLCPP_INFO( + context->node()->get_logger(), + "Requesting replan for [%s] after finishing a schedule override", + context->requester_id().c_str()); + context->request_replan(); + } + else + { + // Trigger the next step in the sequence + finisher(); + } + }); } +} - static CommandExecution make( - const std::shared_ptr& context, - Data data_, - std::function begin) +auto EasyFullControl::CommandExecution::Implementation::override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold) -> Stubbornness +{ + auto stubborn = std::make_shared(); + if (const auto context = w_context.lock()) { - auto data = std::make_shared(data_); - auto update_fn = [w_context = context->weak_from_this(), data]( - const std::string& map, - Eigen::Vector3d location) + context->worker().schedule( + [ + context, + stubborn, + data = this->data, + identifier = this->identifier, + map = std::move(map), + path = std::move(path), + hold + ](const auto&) { - if (auto context = w_context.lock()) + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) { - data->update_location(context, map, location); + // Don't do anything because this command is finished + return; } - }; - auto identifier = ActivityIdentifier::Implementation::make(update_fn); - CommandExecution cmd; - cmd._pimpl = rmf_utils::make_impl( - Implementation{context, data, begin, nullptr, identifier}); - return cmd; + data->release_stubbornness(); + data->schedule_override = ScheduleOverride::make( + context, map, path, hold, stubborn); + }); } - static Implementation& get(CommandExecution& cmd) - { - return *cmd._pimpl; - } -}; + return Stubbornness::Implementation::make(stubborn); +} + +auto EasyFullControl::CommandExecution::Implementation::make( + const std::shared_ptr& context, + Data data_, + std::function begin) -> CommandExecution +{ + auto data = std::make_shared(data_); + auto update_fn = [w_context = context->weak_from_this(), data]( + const std::string& map, + Eigen::Vector3d location) + { + if (auto context = w_context.lock()) + { + data->update_location(context, map, location); + } + }; + auto identifier = ActivityIdentifier::Implementation::make(update_fn); + + CommandExecution cmd; + cmd._pimpl = rmf_utils::make_impl( + Implementation{context, data, begin, nullptr, identifier}); + return cmd; +} + +auto EasyFullControl::CommandExecution::Implementation::make_hold( + const std::shared_ptr& context, + rmf_traffic::Time expected_time, + rmf_traffic::PlanId plan_id, + std::function finisher) -> CommandExecution +{ + auto update_fn = [ + w_context = context->weak_from_this(), + expected_time, + plan_id + ]( + const std::string& map, + Eigen::Vector3d location) + { + const auto context = w_context.lock(); + if (!context) + return; + + const auto delay = context->now() - expected_time; + context->itinerary().cumulative_delay(plan_id, delay); + if (const auto nav_params = context->nav_params()) + { + if (context->debug_positions) + { + std::cout << "Searching for location from " << __FILE__ << "|" << + __LINE__ << std::endl; + } + nav_params->search_for_location(map, location, *context); + } + }; + auto identifier = ActivityIdentifier::Implementation::make(update_fn); + + CommandExecution cmd; + cmd._pimpl = rmf_utils::make_impl( + Implementation{context, nullptr, nullptr, finisher, identifier}); + return cmd; +} //============================================================================== void EasyFullControl::CommandExecution::finished() @@ -631,26 +709,6 @@ EasyFullControl::EasyFullControl() // Do nothing } -//============================================================================== -class EasyFullControl::Destination::Implementation -{ -public: - std::string map; - Eigen::Vector3d position; - std::optional graph_index; - std::optional speed_limit; - std::optional dock = std::nullopt; - - template - static Destination make(Args&&... args) - { - Destination output; - output._pimpl = rmf_utils::make_impl( - Implementation{std::forward(args)...}); - return output; - } -}; - //============================================================================== const std::string& EasyFullControl::Destination::map() const { @@ -681,6 +739,12 @@ std::optional EasyFullControl::Destination::graph_index() const return _pimpl->graph_index; } +//============================================================================== +std::string EasyFullControl::Destination::name() const +{ + return _pimpl->name; +} + //============================================================================== std::optional EasyFullControl::Destination::speed_limit() const { @@ -693,6 +757,13 @@ std::optional EasyFullControl::Destination::dock() const return _pimpl->dock; } +//============================================================================== +rmf_traffic::agv::Graph::LiftPropertiesPtr +EasyFullControl::Destination::inside_lift() const +{ + return _pimpl->lift; +} + //============================================================================== EasyFullControl::Destination::Destination() { @@ -730,6 +801,8 @@ struct ProgressTracker : std::enable_shared_from_this progress->next(); } }; + + const auto begin = current_activity_impl.begin; if (begin) { @@ -794,28 +867,22 @@ class EasyCommandHandle : public RobotCommandHandle, const std::string& map, Eigen::Vector3d position) const { - if (!nav_params->transforms_to_robot_coords) + auto robot_position = nav_params->to_robot_coordinates(map, position); + if (robot_position.has_value()) { - return position; + return *robot_position; } - const auto tf_it = nav_params->transforms_to_robot_coords->find(map); - if (tf_it == nav_params->transforms_to_robot_coords->end()) + if (const auto context = w_context.lock()) { - const auto context = w_context.lock(); - if (context) - { - RCLCPP_WARN( - context->node()->get_logger(), - "[EasyFullControl] Unable to find robot transform for map [%s] for " - "robot [%s]. We will not apply a transform.", - map.c_str(), - context->requester_id().c_str()); - } - return position; + RCLCPP_WARN( + context->node()->get_logger(), + "[EasyFullControl] Unable to find robot transform for map [%s] for " + "robot [%s]. We will not apply a transform.", + map.c_str(), + context->requester_id().c_str()); } - - return tf_it->second.apply(position); + return position; } std::weak_ptr w_context; @@ -863,7 +930,7 @@ void EasyCommandHandle::stop() //============================================================================== void EasyCommandHandle::follow_new_path( - const std::vector& waypoints, + const std::vector& cmd_waypoints, ArrivalEstimator next_arrival_estimator_, RequestCompleted path_finished_callback_) { @@ -879,7 +946,7 @@ void EasyCommandHandle::follow_new_path( context->requester_id().c_str(), context->itinerary().current_plan_id()); - if (waypoints.empty() || + if (cmd_waypoints.empty() || next_arrival_estimator_ == nullptr || path_finished_callback_ == nullptr) { @@ -901,6 +968,18 @@ void EasyCommandHandle::follow_new_path( return; } const auto& graph = planner->get_configuration().graph(); + std::vector waypoints = cmd_waypoints; + + if (waypoints.size() < 2) + { + // This command doesn't actually want us to go anywhere so we will consider + // it completed right away. But in case the robot is doing something else + // right now, we will command it to stop. + stop(); + path_finished_callback_(); + return; + } + std::optional opt_initial_map; for (const auto& wp : waypoints) { @@ -946,7 +1025,8 @@ void EasyCommandHandle::follow_new_path( { for (const auto& l : current_location) { - if (*wp.graph_index() == l.waypoint() && !l.lane().has_value()) + if (nav_params->in_same_stack(*wp.graph_index(), + l.waypoint()) && !l.lane().has_value()) { if (l.location().has_value()) { @@ -978,16 +1058,16 @@ void EasyCommandHandle::follow_new_path( { for (const auto& l : current_location) { - Eigen::Vector2d p_wp; + Eigen::Vector2d p_l; if (l.location().has_value()) { - p_wp = l.location()->block<2, 1>(0, 0); + p_l = *l.location(); } else { - p_wp = graph.get_waypoint(l.waypoint()).get_location(); + p_l = graph.get_waypoint(l.waypoint()).get_location(); } - const double dist = (*l.location() - p_wp).norm(); + const double dist = (wp.position().block<2, 1>(0, 0) - p_l).norm(); if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1094,44 +1174,70 @@ void EasyCommandHandle::follow_new_path( while (i2 < waypoints.size()) { const auto& wp2 = waypoints[i2]; - if (wp1.graph_index().has_value() && wp2.graph_index().has_value()) + + const bool midlane_wait = + !wp1.graph_index().has_value() && !wp2.graph_index().has_value(); + const bool same_stack = + wp1.graph_index().has_value() && wp2.graph_index().has_value() + && nav_params->in_same_stack(*wp1.graph_index(), *wp2.graph_index()); + + if (same_stack || midlane_wait) { - if (*wp1.graph_index() == *wp2.graph_index()) + target_index = i2; + target_position = wp2.position(); + if (std::abs(wp1.position()[2] - + wp2.position()[2])*180.0 / M_PI < 1e-2) { - target_index = i2; - target_position = wp2.position(); - if (std::abs(wp1.position()[2] - - wp2.position()[2])*180.0 / M_PI < 1e-2) - { - // The plan had a wait between these points. - planned_wait_time += wp2.time() - wp1.time(); - } - ++i2; - continue; + // The plan had a wait between these points. + planned_wait_time += wp2.time() - wp1.time(); } + ++i2; + continue; } break; } } + rmf_traffic::agv::Graph::LiftPropertiesPtr in_lift; + if (wp1.graph_index().has_value()) + { + in_lift = graph.get_waypoint(*wp1.graph_index()).in_lift(); + } + else + { + for (const auto& lift : graph.all_known_lifts()) + { + if (lift->is_in_lift(target_position.block<2, 1>(0, 0))) + { + in_lift = lift; + break; + } + } + } + const auto command_position = to_robot_coordinates(map, target_position); auto destination = EasyFullControl::Destination::Implementation::make( std::move(map), command_position, wp1.graph_index(), - speed_limit); + nav_params->get_vertex_name(graph, wp1.graph_index()), + speed_limit, + in_lift); + const auto target_p = waypoints.at(target_index).position(); queue.push_back( EasyFullControl::CommandExecution::Implementation::make( context, EasyFullControl::CommandExecution::Implementation::Data{ cmd_wps, cmd_lanes, + target_position.block<2, 1>(0, 0), target_position[2], planned_wait_time, std::nullopt, nav_params, - [next_arrival_estimator_, target_index](rmf_traffic::Duration dt) + [next_arrival_estimator_, target_index, + target_p](rmf_traffic::Duration dt) { next_arrival_estimator_(target_index, dt); } @@ -1260,13 +1366,17 @@ void EasyCommandHandle::dock( const double dist = (p1 - p0).norm(); const auto& traits = planner->get_configuration().vehicle_traits(); const double v = std::max(traits.linear().get_nominal_velocity(), 0.001); - const double dt = dist / v; - const rmf_traffic::Time expected_arrival = - context->now() + rmf_traffic::time::from_seconds(dt); + const auto dt = rmf_traffic::time::from_seconds(dist / v); + const auto& itin = context->itinerary(); + const auto now = context->now(); + const auto initial_delay = itin.cumulative_delay(itin.current_plan_id()) + .value_or(rmf_traffic::Duration(0)); + const rmf_traffic::Time expected_arrival = now + dt - initial_delay; auto data = EasyFullControl::CommandExecution::Implementation::Data{ {i0, i1}, {*found_lane}, + p1, std::nullopt, rmf_traffic::Duration(0), std::nullopt, @@ -1280,11 +1390,23 @@ void EasyCommandHandle::dock( return; } - const rmf_traffic::Time now = context->now(); - const auto updated_arrival = now + dt; - const auto delay = updated_arrival - expected_arrival; - context->itinerary().cumulative_delay( - plan_id, delay, std::chrono::seconds(1)); + context->worker().schedule([ + w = context->weak_from_this(), + expected_arrival, + plan_id, + dt + ](const auto&) + { + const auto context = w.lock(); + if (!context) + return; + + const rmf_traffic::Time now = context->now(); + const auto updated_arrival = now + dt; + const auto delay = updated_arrival - expected_arrival; + context->itinerary().cumulative_delay( + plan_id, delay, std::chrono::seconds(1)); + }); } }; @@ -1304,7 +1426,9 @@ void EasyCommandHandle::dock( wp1.get_map_name(), command_position, i1, + nav_params->get_vertex_name(graph, i1), lane.properties().speed_limit(), + wp1.in_lift(), dock_name_); auto cmd = EasyFullControl::CommandExecution::Implementation::make( @@ -1318,6 +1442,7 @@ void EasyCommandHandle::dock( { handle_nav_request(destination, execution); }); + this->current_progress = ProgressTracker::make( {std::move(cmd)}, std::move(docking_finished_callback_)); @@ -1431,33 +1556,17 @@ void EasyFullControl::EasyRobotUpdateHandle::update( ActivityIdentifier::Implementation::get(*current_activity).update_fn; if (update_fn) { - update_fn(state.map(), position); + update_fn( + state.map(), position); return; } } - auto planner = context->planner(); - if (!planner) + if (context->debug_positions) { - RCLCPP_ERROR( - context->node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context->requester_id().c_str()); - return; - } - const auto& graph = planner->get_configuration().graph(); - const auto& nav_params = updater->nav_params; - const auto now = context->now(); - auto starts = - nav_params->compute_plan_starts(graph, state.map(), position, now); - if (!starts.empty()) - { - context->set_location(std::move(starts)); - } - else - { - context->set_lost(Location { now, state.map(), position }); + std::cout << "Searching for location from " << __FILE__ << "|" << __LINE__ << std::endl; } + updater->nav_params->search_for_location(state.map(), position, *context); }); } @@ -1559,6 +1668,7 @@ class EasyFullControl::FleetConfiguration::Implementation double default_max_merge_waypoint_distance; double default_max_merge_lane_distance; double default_min_lane_length; + std::unordered_map lift_emergency_levels; }; //============================================================================== @@ -1612,7 +1722,8 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( default_responsive_wait, std::move(default_max_merge_waypoint_distance), std::move(default_max_merge_lane_distance), - std::move(default_min_lane_length) + std::move(default_min_lane_length), + {} })) { // Do nothing @@ -1893,8 +2004,10 @@ EasyFullControl::FleetConfiguration::from_config_files( rmf_task::ConstRequestFactoryPtr finishing_request; if (finishing_request_string == "charge") { - finishing_request = + auto charge_factory = std::make_shared(); + charge_factory->set_indefinite(true); + finishing_request = charge_factory; std::cout << "Fleet is configured to perform ChargeBattery as finishing request" << std::endl; @@ -1902,7 +2015,8 @@ EasyFullControl::FleetConfiguration::from_config_files( else if (finishing_request_string == "park") { finishing_request = - std::make_shared(); + std::make_shared( + "idle", nullptr); std::cout << "Fleet is configured to perform ParkRobot as finishing request" << std::endl; @@ -2060,7 +2174,7 @@ EasyFullControl::FleetConfiguration::from_config_files( const auto robot_name = robot.first.as(); const YAML::Node& robot_config_yaml = robot.second; - if (!robot_config_yaml.IsMap()) + if (!robot_config_yaml.IsMap() && !robot_config_yaml.IsNull()) { std::cerr << "Entry for [" << robot_name << "] in [robots] dictionary is not " @@ -2068,65 +2182,90 @@ EasyFullControl::FleetConfiguration::from_config_files( return std::nullopt; } - std::string charger; - const YAML::Node& charger_yaml = robot_config_yaml["charger"]; - if (charger_yaml) - { - charger = charger_yaml.as(); - } - else + if (robot_config_yaml.IsMap()) { - std::cerr - << "No [charger] listed for [" << robot_name << "] in the config " - << "file [" << config_file << "]. A robot configuration cannot be " - << "made for the robot." << std::endl; - return std::nullopt; - } + std::vector chargers; + const YAML::Node& charger_yaml = robot_config_yaml["charger"]; + if (charger_yaml) + { + chargers.push_back(charger_yaml.as()); + } - const YAML::Node& responsive_wait_yaml = - robot_config_yaml["responsive_wait"]; - std::optional responsive_wait = std::nullopt; - if (responsive_wait_yaml) - { - responsive_wait = responsive_wait_yaml.as(); - } + const YAML::Node& responsive_wait_yaml = + robot_config_yaml["responsive_wait"]; + std::optional responsive_wait = std::nullopt; + if (responsive_wait_yaml) + { + responsive_wait = responsive_wait_yaml.as(); + } - const YAML::Node& max_merge_waypoint_distance_yaml = - robot_config_yaml["max_merge_waypoint_distance"]; - std::optional max_merge_waypoint_distance = std::nullopt; - if (max_merge_waypoint_distance_yaml) - { - max_merge_waypoint_distance = - max_merge_waypoint_distance_yaml.as(); - } + const YAML::Node& max_merge_waypoint_distance_yaml = + robot_config_yaml["max_merge_waypoint_distance"]; + std::optional max_merge_waypoint_distance = std::nullopt; + if (max_merge_waypoint_distance_yaml) + { + max_merge_waypoint_distance = + max_merge_waypoint_distance_yaml.as(); + } - const YAML::Node& max_merge_lane_distance_yaml = - robot_config_yaml["max_merge_lane_distance"]; - std::optional max_merge_lane_distance = std::nullopt; - if (max_merge_lane_distance_yaml) - { - max_merge_lane_distance = max_merge_lane_distance_yaml.as(); - } + const YAML::Node& max_merge_lane_distance_yaml = + robot_config_yaml["max_merge_lane_distance"]; + std::optional max_merge_lane_distance = std::nullopt; + if (max_merge_lane_distance_yaml) + { + max_merge_lane_distance = max_merge_lane_distance_yaml.as(); + } - const YAML::Node& min_lane_length_yaml = - robot_config_yaml["min_lane_length"]; - std::optional min_lane_length = std::nullopt; - if (min_lane_length_yaml) - { - min_lane_length = min_lane_length_yaml.as(); - } + const YAML::Node& min_lane_length_yaml = + robot_config_yaml["min_lane_length"]; + std::optional min_lane_length = std::nullopt; + if (min_lane_length_yaml) + { + min_lane_length = min_lane_length_yaml.as(); + } - auto config = RobotConfiguration({std::move(charger)}, + auto config = RobotConfiguration( + std::move(chargers), responsive_wait, max_merge_waypoint_distance, max_merge_lane_distance, min_lane_length); - known_robot_configurations.insert_or_assign(robot_name, config); + known_robot_configurations.insert_or_assign(robot_name, config); + } + else + { + auto config = RobotConfiguration({}); + known_robot_configurations.insert_or_assign(robot_name, config); + } } } } - return FleetConfiguration( + std::unordered_map lift_emergency_levels; + const YAML::Node& lift_emergency_levels_yaml = + rmf_fleet["lift_emergency_levels"]; + if (lift_emergency_levels_yaml) + { + if (!lift_emergency_levels_yaml.IsMap()) + { + std::cerr + << "[lift_emergency_levels] element is not a map in the config file [" + << config_file << "] so we cannot parse what level each lift will go " + << "to in an emergency." << std::endl; + return std::nullopt; + } + else + { + for (const auto& lift : lift_emergency_levels_yaml) + { + auto lift_name = lift.first.as(); + auto level_name = lift.second.as(); + lift_emergency_levels[std::move(lift_name)] = std::move(level_name); + } + } + } + + auto config = FleetConfiguration( fleet_name, std::move(tf_dict), std::move(known_robot_configurations), @@ -2150,6 +2289,8 @@ EasyFullControl::FleetConfiguration::from_config_files( default_max_merge_waypoint_distance, default_max_merge_lane_distance, default_min_lane_length); + config.change_lift_emergency_levels() = lift_emergency_levels; + return config; } //============================================================================== @@ -2495,6 +2636,29 @@ void EasyFullControl::FleetConfiguration::set_default_min_lane_length( _pimpl->default_min_lane_length = distance; } +//============================================================================== +void EasyFullControl::FleetConfiguration::set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name) +{ + _pimpl->lift_emergency_levels[std::move(lift_name)] = + std::move(emergency_level_name); +} + +//============================================================================== +std::unordered_map& +EasyFullControl::FleetConfiguration::change_lift_emergency_levels() +{ + return _pimpl->lift_emergency_levels; +} + +//============================================================================== +const std::unordered_map& +EasyFullControl::FleetConfiguration::lift_emergency_levels() const +{ + return _pimpl->lift_emergency_levels; +} + //============================================================================== using EasyCommandHandlePtr = std::shared_ptr; @@ -2525,6 +2689,28 @@ auto EasyFullControl::add_robot( auto easy_updater = EasyRobotUpdateHandle::Implementation::make( robot_nav_params, worker); + LocalizationRequest localization = nullptr; + if (callbacks.localize()) + { + localization = [ + inner = callbacks.localize(), + nav_params = robot_nav_params + ](Destination estimate, CommandExecution execution) + { + auto robot_position = nav_params->to_robot_coordinates( + estimate.map(), + estimate.position()); + if (robot_position.has_value()) + { + auto transformed_estimate = estimate; + Destination::Implementation::get(transformed_estimate) + .position = *robot_position; + + inner(transformed_estimate, execution); + } + }; + } + const auto& fleet_impl = FleetUpdateHandle::Implementation::get(*_pimpl->fleet_handle); const auto& planner = *fleet_impl.planner; @@ -2549,36 +2735,39 @@ auto EasyFullControl::add_robot( return nullptr; } - if (configuration.compatible_chargers().size() != 1) + if (configuration.compatible_chargers().size() > 1) { - RCLCPP_ERROR( + RCLCPP_WARN( node->get_logger(), - "Robot [%s] is configured to have %lu chargers, but we require it to " - "have exactly 1. It will not be added to the fleet.", + "Robot [%s] is configured to have %lu chargers, but we will only make " + "use of the first one. Making use of multiple chargers will be added in " + "a future version of RMF.", robot_name.c_str(), configuration.compatible_chargers().size()); - - _pimpl->cmd_handles.erase(robot_name); - return nullptr; } - const auto& charger_name = configuration.compatible_chargers().front(); - const auto* charger_wp = graph.find_waypoint(charger_name); - if (!charger_wp) + std::optional charger_index; + if (!configuration.compatible_chargers().empty()) { - RCLCPP_ERROR( - node->get_logger(), - "Cannot find a waypoint named [%s] in the navigation graph of fleet [%s] " - "needed for the charging point of robot [%s]. We will not add the robot " - "to the fleet.", - charger_name.c_str(), - fleet_name.c_str(), - robot_name.c_str()); + const auto& charger_name = configuration.compatible_chargers().front(); + const auto* charger_wp = graph.find_waypoint(charger_name); + if (!charger_wp) + { + RCLCPP_ERROR( + node->get_logger(), + "Cannot find a waypoint named [%s] in the navigation graph of fleet " + "[%s] needed for the charging point of robot [%s]. We will not add the " + "robot to the fleet.", + charger_name.c_str(), + fleet_name.c_str(), + robot_name.c_str()); + + _pimpl->cmd_handles.erase(robot_name); + return nullptr; + } - _pimpl->cmd_handles.erase(robot_name); - return nullptr; + charger_index = charger_wp->index(); } - const std::size_t charger_index = charger_wp->index(); rmf_traffic::Time now = std::chrono::steady_clock::time_point( std::chrono::nanoseconds(node->now().nanoseconds())); @@ -2614,6 +2803,7 @@ auto EasyFullControl::add_robot( robot_nav_params->min_lane_length = *configuration.min_lane_length(); } + robot_nav_params->find_stacked_vertices(graph); const Eigen::Vector3d position = *position_opt; auto starts = robot_nav_params->compute_plan_starts( graph, initial_state.map(), position, now); @@ -2679,6 +2869,7 @@ auto EasyFullControl::add_robot( fleet_name = fleet_name, charger_index, action_executor = callbacks.action_executor(), + localization = std::move(localization), nav_params = robot_nav_params, enable_responsive_wait ](const RobotUpdateHandlePtr& updater) @@ -2696,6 +2887,7 @@ auto EasyFullControl::add_robot( fleet_name, charger_index, action_executor, + localization, context, nav_params, enable_responsive_wait @@ -2706,7 +2898,11 @@ auto EasyFullControl::add_robot( EasyRobotUpdateHandle::Implementation::get(*easy_updater) .updater->handle = handle; handle->set_action_executor(action_executor); - handle->set_charger_waypoint(charger_index); + context->set_localization(localization); + if (charger_index.has_value()) + { + handle->set_charger_waypoint(*charger_index); + } handle->enable_responsive_wait(enable_responsive_wait); RCLCPP_INFO( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 61365bf03..2e148d22d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -169,18 +169,6 @@ void FleetUpdateHandle::Implementation::dock_summary_cb( return; } -//============================================================================== -std::string FleetUpdateHandle::Implementation::make_error_str( - uint64_t code, std::string category, std::string detail) const -{ - nlohmann::json error; - error["code"] = code; - error["category"] = std::move(category); - error["detail"] = std::move(detail); - - return error.dump(); -} - //============================================================================== std::shared_ptr FleetUpdateHandle::Implementation::convert( const std::string& task_id, @@ -315,6 +303,131 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( return new_request; } +//============================================================================== +class AllocateTasks : public std::enable_shared_from_this +{ +public: + + template + void operator()(const Subscriber& s) + { + std::vector errors; + auto assignments = run(errors); + s.on_next(Result{std::move(assignments), std::move(errors)}); + s.on_completed(); + } + + std::optional run(std::vector& errors) + { + std::string id = ""; + + if (new_request) + { + expect.pending_requests.push_back(new_request); + id = new_request->booking()->id(); + } + + RCLCPP_INFO( + node->get_logger(), + "Planning for [%ld] robot(s) and [%ld] request(s)", + expect.states.size(), + expect.pending_requests.size()); + + // Generate new task assignments + const auto result = task_planner.plan( + rmf_traffic_ros2::convert(node->now()), + expect.states, + expect.pending_requests); + + auto assignments_ptr = std::get_if< + rmf_task::TaskPlanner::Assignments>(&result); + + if (!assignments_ptr) + { + auto error = std::get_if< + rmf_task::TaskPlanner::TaskPlannerError>(&result); + + if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery) + { + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + + "] due to insufficient initial battery charge for all robots in this " + "fleet."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } + + else if (*error == + rmf_task::TaskPlanner::TaskPlannerError::limited_capacity) + { + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "] due to insufficient battery capacity to accommodate one or more " + "requests by any of the robots in this fleet."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } + + else + { + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "]"; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } + + return std::nullopt; + } + + const auto assignments = *assignments_ptr; + + if (assignments.empty()) + { + RCLCPP_ERROR( + node->get_logger(), + "[TaskPlanner] Failed to compute assignments for task_id [%s]", + id.c_str()); + + return std::nullopt; + } + + return assignments; + } + + struct Result + { + std::optional assignments; + std::vector errors; + }; + + AllocateTasks( + rmf_task::ConstRequestPtr new_request_, + Expectations expect_, + rmf_task::TaskPlanner task_planner_, + std::shared_ptr node_) + : new_request(std::move(new_request_)), + expect(std::move(expect_)), + task_planner(std::move(task_planner_)), + node(std::move(node_)) + { + // Do nothing + } + + rmf_task::ConstRequestPtr new_request; + Expectations expect; + rmf_task::TaskPlanner task_planner; + std::shared_ptr node; +}; + + //============================================================================== void FleetUpdateHandle::Implementation::bid_notice_cb( const BidNoticeMsg& bid_notice, @@ -358,7 +471,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const auto request_msg = nlohmann::json::parse(bid_notice.request); static const auto request_validator = - nlohmann::json_schema::json_validator(rmf_api_msgs::schemas::task_request); + nlohmann::json_schema::json_validator( + rmf_api_msgs::schemas::task_request); try { @@ -390,104 +504,124 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( }); } - // TODO(MXG): Make the task planning asynchronous. The worker should schedule - // a job to perform the planning which should then spawn a job to save the - // plan result and respond. I started to refactor allocate_tasks(~) to make it - // async, but I will save the remaining effort for later, when there is more - // time to spare. - auto allocation_result = allocate_tasks(new_request, &errors); - if (!allocation_result.has_value()) - return respond({std::nullopt, std::move(errors)}); + calculate_bid = std::make_shared( + new_request, + aggregate_expectations(), + *task_planner, + node); - const auto& assignments = allocation_result.value(); + auto receive_allocation = [w = weak_self, respond, task_id]( + AllocateTasks::Result result) + { + const auto self = w.lock(); + if (!self) + return; - const double cost = task_planner->compute_cost(assignments); + auto allocation_result = result.assignments; + if (!allocation_result.has_value()) + return respond({std::nullopt, std::move(result.errors)}); - // Display computed assignments for debugging - std::stringstream debug_stream; - debug_stream << "Cost: " << cost << std::endl; - for (std::size_t i = 0; i < assignments.size(); ++i) - { - debug_stream << "--Agent: " << i << std::endl; - for (const auto& a : assignments[i]) - { - const auto& s = a.finish_state(); - const double request_seconds = - a.request()->booking()->earliest_start_time().time_since_epoch().count() - /1e9; - const double start_seconds = - a.deployment_time().time_since_epoch().count()/1e9; - const rmf_traffic::Time finish_time = s.time().value(); - const double finish_seconds = finish_time.time_since_epoch().count()/1e9; - debug_stream << " <" << a.request()->booking()->id() << ": " << - request_seconds - << ", " << start_seconds - << ", "<< finish_seconds << ", " << s.battery_soc().value() - << "%>" << std::endl; - } - } - debug_stream << " ----------------------" << std::endl; + const auto& assignments = allocation_result.value(); - RCLCPP_DEBUG(node->get_logger(), "%s", debug_stream.str().c_str()); + const double cost = self->_pimpl->task_planner->compute_cost(assignments); - // Map robot index to name to populate robot_name in BidProposal - std::unordered_map robot_name_map; - std::size_t index = 0; - for (const auto& t : task_managers) - { - robot_name_map.insert({index, t.first->name()}); - ++index; - } + // Display computed assignments for debugging + std::stringstream debug_stream; + debug_stream << "Cost: " << cost << std::endl; + for (std::size_t i = 0; i < assignments.size(); ++i) + { + debug_stream << "--Agent: " << i << std::endl; + for (const auto& a : assignments[i]) + { + const auto& s = a.finish_state(); + const double request_seconds = + a.request()->booking()->earliest_start_time().time_since_epoch(). + count() + /1e9; + const double start_seconds = + a.deployment_time().time_since_epoch().count()/1e9; + const rmf_traffic::Time finish_time = s.time().value(); + const double finish_seconds = finish_time.time_since_epoch().count()/ + 1e9; + debug_stream << " <" << a.request()->booking()->id() << ": " << + request_seconds + << ", " << start_seconds + << ", "<< finish_seconds << ", " << + s.battery_soc().value() + << "%>" << std::endl; + } + } + debug_stream << " ----------------------" << std::endl; - std::optional robot_name; - std::optional finish_time; - index = 0; - for (const auto& agent : assignments) - { - for (const auto& assignment : agent) - { - if (assignment.request()->booking()->id() == task_id) + RCLCPP_DEBUG( + self->_pimpl->node->get_logger(), + "%s", + debug_stream.str().c_str()); + + // Map robot index to name to populate robot_name in BidProposal + std::unordered_map robot_name_map; + std::size_t index = 0; + for (const auto& t : self->_pimpl->task_managers) { - finish_time = assignment.finish_state().time().value(); - if (robot_name_map.find(index) != robot_name_map.end()) - robot_name = robot_name_map[index]; - break; + robot_name_map.insert({index, t.first->name()}); + ++index; } - } - ++index; - } - if (!robot_name.has_value() || !finish_time.has_value()) - { - errors.push_back( - make_error_str( - 13, "Internal bug", - "Failed to find robot_name or finish_time after allocating task. " - "Please report this bug to the RMF developers.")); + std::optional robot_name; + std::optional finish_time; + index = 0; + for (const auto& agent : assignments) + { + for (const auto& assignment : agent) + { + if (assignment.request()->booking()->id() == task_id) + { + finish_time = assignment.finish_state().time().value(); + if (robot_name_map.find(index) != robot_name_map.end()) + robot_name = robot_name_map[index]; + break; + } + } + ++index; + } - return respond({std::nullopt, std::move(errors)}); - } + if (!robot_name.has_value() || !finish_time.has_value()) + { + result.errors.push_back( + make_error_str( + 13, "Internal bug", + "Failed to find robot_name or finish_time after allocating task. " + "Please report this bug to the RMF developers.")); - // Publish BidProposal - respond( - { - rmf_task_ros2::bidding::Response::Proposal{ - name, - *robot_name, - current_assignment_cost, - cost, - *finish_time - }, - std::move(errors) - }); + return respond({std::nullopt, std::move(result.errors)}); + } - RCLCPP_INFO( - node->get_logger(), - "Submitted BidProposal to accommodate task [%s] by robot [%s] with new cost [%f]", - task_id.c_str(), robot_name->c_str(), cost); + // Publish BidProposal + respond( + { + rmf_task_ros2::bidding::Response::Proposal{ + self->_pimpl->name, + *robot_name, + self->_pimpl->current_assignment_cost, + cost, + *finish_time + }, + std::move(result.errors) + }); + + RCLCPP_INFO( + self->_pimpl->node->get_logger(), + "Submitted BidProposal to accommodate task [%s] by robot [%s] with new cost [%f]", + task_id.c_str(), robot_name->c_str(), cost); + + // Store assignments in internal map + self->_pimpl->bid_notice_assignments.insert({task_id, assignments}); + }; - // Store assignments in internal map - bid_notice_assignments.insert({task_id, assignments}); + calculate_bid_subscription = rmf_rxcpp::make_job( + calculate_bid) + .observe_on(rxcpp::identity_same_worker(worker)) + .subscribe(receive_allocation); } //============================================================================== @@ -619,7 +753,10 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( // TODO: This replanning is blocking the main thread. Instead, the // replanning should run on a separate worker and then deliver the // result back to the main worker. - const auto replan_results = allocate_tasks(request, &dispatch_ack.errors); + const auto replan_results = AllocateTasks( + nullptr, aggregate_expectations(), *task_planner, node) + .run(dispatch_ack.errors); + if (!replan_results) { std::string error_str = @@ -639,22 +776,24 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( // rxcpp worker. Hence, no new tasks would have started during this // replanning. } - - std::size_t index = 0; - for (auto& t : task_managers) + else { - t.second->set_queue(assignments[index]); - ++index; - } + std::size_t index = 0; + for (auto& t : task_managers) + { + t.second->set_queue(assignments[index]); + ++index; + } - current_assignment_cost = task_planner->compute_cost(assignments); - dispatch_ack.success = true; - dispatch_ack_pub->publish(dispatch_ack); + current_assignment_cost = task_planner->compute_cost(assignments); + dispatch_ack.success = true; + dispatch_ack_pub->publish(dispatch_ack); - RCLCPP_INFO( - node->get_logger(), - "Assignments updated for robots in fleet [%s] to accommodate task_id [%s]", - name.c_str(), task_id.c_str()); + RCLCPP_INFO( + node->get_logger(), + "TaskAssignments updated for robots in fleet [%s] to accommodate task_id [%s]", + name.c_str(), task_id.c_str()); + } } else if (msg->type == DispatchCmdMsg::TYPE_REMOVE) { @@ -681,7 +820,10 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( { // Re-plan assignments while ignoring request for task to be cancelled std::vector errors; - const auto replan_results = allocate_tasks(nullptr, &errors); + const auto replan_results = AllocateTasks( + nullptr, aggregate_expectations(), *task_planner, node) + .run(errors); + if (!replan_results.has_value()) { std::stringstream ss; @@ -718,7 +860,7 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( RCLCPP_INFO( node->get_logger(), - "Task with task_id [%s] has successfully been cancelled. Assignments " + "Task with task_id [%s] has successfully been cancelled. TaskAssignments " "updated for robots in fleet [%s].", task_id.c_str(), name.c_str()); } @@ -741,7 +883,7 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( //============================================================================== auto FleetUpdateHandle::Implementation::is_valid_assignments( - Assignments& assignments) const -> bool + TaskAssignments& assignments) const -> bool { std::unordered_set executed_tasks; for (const auto& [context, mgr] : task_managers) @@ -1016,6 +1158,160 @@ void FleetUpdateHandle::Implementation::update_fleet_logs() const } } +//============================================================================== +void FleetUpdateHandle::Implementation::handle_emergency( + const bool is_emergency) +{ + if (is_emergency == emergency_active) + return; + + emergency_active = is_emergency; + if (is_emergency) + { + update_emergency_planner(); + } + + for (const auto& [context, _] : task_managers) + { + context->_set_emergency(is_emergency); + } + emergency_publisher.get_subscriber().on_next(is_emergency); +} + +//============================================================================== +namespace { +class EmergencyLaneCloser : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + void execute(const DoorOpen&) override {} + void execute(const DoorClose&) override {} + void execute(const LiftSessionEnd&) override {} + void execute(const LiftMove&) override {} + void execute(const Wait&) override {} + void execute(const Dock& dock) override {} + void execute(const LiftSessionBegin& info) override + { + lift = info.lift_name(); + enter = true; + } + void execute(const LiftDoorOpen& info) override + { + lift = info.lift_name(); + exit = true; + } + + std::string lift; + bool enter = false; + bool exit = false; +}; +} // anonymous namespace + +//============================================================================== +std::vector find_emergency_lift_closures( + const rmf_traffic::agv::Graph& graph, + const std::unordered_map& emergency_level_for_lift) +{ + std::vector closures; + for (std::size_t i = 0; i < graph.num_lanes(); ++i) + { + EmergencyLaneCloser executor; + const auto& lane = graph.get_lane(i); + if (const auto* event = lane.entry().event()) + event->execute(executor); + + if (const auto* event = lane.exit().event()) + event->execute(executor); + + const auto wp0 = lane.entry().waypoint_index(); + const auto wp1 = lane.exit().waypoint_index(); + if (executor.enter) + { + if (emergency_level_for_lift.count(executor.lift) > 0) + { + closures.push_back(i); + } + } + else if (executor.exit) + { + const auto l_it = emergency_level_for_lift.find(executor.lift); + if (l_it != emergency_level_for_lift.end()) + { + const auto& map = graph.get_waypoint( + lane.exit().waypoint_index()).get_map_name(); + if (l_it->second != map) + { + closures.push_back(i); + } + } + } + } + + return closures; +} + +//============================================================================== +void FleetUpdateHandle::Implementation::update_emergency_planner() +{ + const auto& config = (*planner)->get_configuration(); + const auto lift_closures = find_emergency_lift_closures( + config.graph(), + emergency_level_for_lift); + const auto& normal_closures = config.lane_closures(); + auto emergency_closures = normal_closures; + for (const std::size_t lane : lift_closures) + { + emergency_closures.close(lane); + } + + auto emergency_config = config; + emergency_config.lane_closures(std::move(emergency_closures)); + + *emergency_planner = std::make_shared( + emergency_config, rmf_traffic::agv::Planner::Options(nullptr)); +} + +//============================================================================== +void FleetUpdateHandle::Implementation::update_charging_assignments( + const ChargingAssignments& charging) +{ + if (charging.fleet_name != name) + return; + + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] received new charging assignments", + name.c_str()); + + for (const ChargingAssignment& assignment : charging.assignments) + { + bool found_robot = false; + for (const auto& [context, _] : task_managers) + { + if (context->name() != assignment.robot_name) + continue; + + const rmf_traffic::agv::Graph& graph = context->navigation_graph(); + const auto wp = graph.find_waypoint(assignment.waypoint_name); + if (!wp) + { + RCLCPP_ERROR( + node->get_logger(), + "Cannot change charging waypoint for [%s] to [%s] because it does " + "not exist in the graph", + context->requester_id().c_str(), + assignment.waypoint_name.c_str()); + } + const bool wait_for_charger = assignment.mode == assignment.MODE_WAIT; + context->_set_charging(wp->index(), wait_for_charger); + } + + if (!found_robot) + { + unregistered_charging_assignments[assignment.robot_name] = assignment; + } + } +} + //============================================================================== nlohmann::json_schema::json_validator FleetUpdateHandle::Implementation::make_validator( @@ -1197,106 +1493,6 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const return expect; } -//============================================================================== -auto FleetUpdateHandle::Implementation::allocate_tasks( - rmf_task::ConstRequestPtr new_request, - std::vector* errors, - std::optional expectations) const -> std::optional -{ - // Collate robot states, constraints and combine new requestptr with - // requestptr of non-charging tasks in task manager queues - auto expect = expectations.has_value() ? expectations.value() : - aggregate_expectations(); - std::string id = ""; - - if (new_request) - { - expect.pending_requests.push_back(new_request); - id = new_request->booking()->id(); - } - - RCLCPP_INFO( - node->get_logger(), - "Planning for [%ld] robot(s) and [%ld] request(s)", - expect.states.size(), - expect.pending_requests.size()); - - // Generate new task assignments - const auto result = task_planner->plan( - rmf_traffic_ros2::convert(node->now()), - expect.states, - expect.pending_requests); - - auto assignments_ptr = std::get_if< - rmf_task::TaskPlanner::Assignments>(&result); - - if (!assignments_ptr) - { - auto error = std::get_if< - rmf_task::TaskPlanner::TaskPlannerError>(&result); - - if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery) - { - std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id - + "] due to insufficient initial battery charge for all robots in this " - "fleet."; - - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - if (errors) - { - errors->push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - } - } - - else if (*error == - rmf_task::TaskPlanner::TaskPlannerError::limited_capacity) - { - std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id - + "] due to insufficient battery capacity to accommodate one or more " - "requests by any of the robots in this fleet."; - - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - if (errors) - { - errors->push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - } - } - - else - { - std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id + "]"; - - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - if (errors) - { - errors->push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - } - } - - return std::nullopt; - } - - const auto assignments = *assignments_ptr; - - if (assignments.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "[TaskPlanner] Failed to compute assignments for task_id [%s]", - id.c_str()); - - return std::nullopt; - } - - return assignments; -} - //============================================================================== const std::string& FleetUpdateHandle::fleet_name() const { @@ -1355,23 +1551,20 @@ void FleetUpdateHandle::add_robot( rmf_task::State state; state.load_basic(start[0], charger_wp.value(), 1.0); - auto context = std::make_shared( - RobotContext - { - std::move(command), - std::move(start), - std::move(participant), - fleet->_pimpl->mirror, - fleet->_pimpl->planner, - fleet->_pimpl->activation.task, - fleet->_pimpl->task_parameters, - fleet->_pimpl->node, - fleet->_pimpl->worker, - fleet->_pimpl->default_maximum_delay, - state, - fleet->_pimpl->task_planner - } - ); + auto context = RobotContext::make( + std::move(command), + std::move(start), + std::move(participant), + fleet->_pimpl->mirror, + fleet->_pimpl->planner, + fleet->_pimpl->emergency_planner, + fleet->_pimpl->activation.task, + fleet->_pimpl->task_parameters, + fleet->_pimpl->node, + fleet->_pimpl->worker, + fleet->_pimpl->default_maximum_delay, + state, + fleet->_pimpl->task_planner); // We schedule the following operations on the worker to make sure we do not // have a multiple read/write race condition on the FleetUpdateHandle. @@ -1389,6 +1582,10 @@ void FleetUpdateHandle::add_robot( if (!node) return; + if (fleet->_pimpl->emergency_active) + { + context->_set_emergency(true); + } // TODO(MXG): We need to perform this test because we do not currently // support the distributed negotiation in unit test environments. We @@ -1408,21 +1605,11 @@ void FleetUpdateHandle::add_robot( { if (const auto c = w.lock()) { + const auto& graph = c->navigation_graph(); std::stringstream ss; ss << "Failed negotiation for [" << c->requester_id() - << "] with these starts:"; - for (const auto& l : c->location()) - { - ss << "\n -- t:" << l.time().time_since_epoch().count() - << " | wp:" << l.waypoint() << " | ori:" - << l.orientation(); - if (l.location().has_value()) - { - const auto& p = *l.location(); - ss << " | pos:(" << p.x() << ", " << p.y() << ")"; - } - } - ss << "\n -- Fin --"; + << "] with these starts:" + << print_starts(c->location(), graph); std::cout << ss.str() << std::endl; auto& last_time = *last_interrupt_time; @@ -1434,11 +1621,14 @@ void FleetUpdateHandle::add_robot( } last_time = now; - RCLCPP_INFO( - c->node()->get_logger(), - "Requesting replan for [%s] because it failed to negotiate", - c->requester_id().c_str()); - c->request_replan(); + if (!c->is_stubborn()) + { + RCLCPP_INFO( + c->node()->get_logger(), + "Requesting replan for [%s] because it failed to negotiate", + c->requester_id().c_str()); + c->request_replan(); + } } }); context->_set_negotiation_license(std::move(negotiation_license)); @@ -1456,11 +1646,39 @@ void FleetUpdateHandle::add_robot( if (fleet->_pimpl->broadcast_client) broadcast_client = fleet->_pimpl->broadcast_client; - fleet->_pimpl->task_managers.insert({context, - TaskManager::make( - context, - broadcast_client, - std::weak_ptr(fleet))}); + const auto mgr = TaskManager::make( + context, + broadcast_client, + std::weak_ptr(fleet)); + + fleet->_pimpl->task_managers.insert({context, mgr}); + + const auto c_it = fleet->_pimpl + ->unregistered_charging_assignments.find(context->name()); + if (c_it != fleet->_pimpl->unregistered_charging_assignments.end()) + { + const auto& charging = c_it->second; + const auto& graph = context->navigation_graph(); + const auto* wp = graph.find_waypoint(charging.waypoint_name); + if (!wp) + { + RCLCPP_ERROR( + fleet->_pimpl->node->get_logger(), + "Cannot find a waypoing named [%s] for robot [%s], which was " + "requested as its charging point", + charging.waypoint_name.c_str(), + context->requester_id().c_str()); + } + else + { + context->_set_charging( + wp->index(), + charging.mode == charging.MODE_WAIT); + } + fleet->_pimpl->unregistered_charging_assignments.erase(c_it); + } + + mgr->set_idle_task(fleet->_pimpl->idle_task); // -- Calling the handle_cb should always happen last -- if (handle_cb) @@ -1578,16 +1796,12 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) if (!self) return; - const auto& current_lane_closures = - (*self->_pimpl->planner)->get_configuration().lane_closures(); - bool any_changes = false; for (const auto& lane : lane_indices) { - if (current_lane_closures.is_open(lane)) + if (self->_pimpl->closed_lanes.insert(lane).second) { any_changes = true; - break; } } @@ -1602,14 +1816,17 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) for (const auto& lane : lane_indices) { new_lane_closures.close(lane); - // Bookkeeping - self->_pimpl->closed_lanes.insert(lane); } *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); + if (self->_pimpl->emergency_active) + { + self->_pimpl->update_emergency_planner(); + } + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); self->_pimpl->publish_lane_states(); @@ -1631,16 +1848,17 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) if (!self) return; - const auto& current_lane_closures = - (*self->_pimpl->planner)->get_configuration().lane_closures(); - + // Note that this approach to tracking whether to open a lane will make + // it impossible for an external user to open a lane which has been closed + // by the emergency_level_for_lift behavior. For now this is intentional, + // but in future implementations we may want to allow users to decide if + // that is desirable behavior. bool any_changes = false; for (const auto& lane : lane_indices) { - if (current_lane_closures.is_closed(lane)) + if (self->_pimpl->closed_lanes.erase(lane) > 0) { any_changes = true; - break; } } @@ -1655,19 +1873,31 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) for (const auto& lane : lane_indices) { new_lane_closures.open(lane); - // Bookkeeping - self->_pimpl->closed_lanes.erase(lane); } *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); + if (self->_pimpl->emergency_active) + { + self->_pimpl->update_emergency_planner(); + } + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); self->_pimpl->publish_lane_states(); }); } +//============================================================================== +void FleetUpdateHandle::set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name) +{ + _pimpl->emergency_level_for_lift[std::move(lift_name)] = + std::move(emergency_level_name); +} + //============================================================================== class FleetUpdateHandle::SpeedLimitRequest::Implementation { @@ -2075,7 +2305,7 @@ bool FleetUpdateHandle::set_task_planner_params( double recharge_threshold, double recharge_soc, bool account_for_battery_drain, - rmf_task::ConstRequestFactoryPtr finishing_request) + rmf_task::ConstRequestFactoryPtr idle_task) { if (battery_system && motion_sink && @@ -2101,15 +2331,19 @@ bool FleetUpdateHandle::set_task_planner_params( const rmf_task::TaskPlanner::Options options{ false, nullptr, - finishing_request}; + // The finishing request is no longer handled by the planner, we handle + // it separately as a waiting behavior now. + nullptr}; _pimpl->worker.schedule( - [w = weak_from_this(), task_config, options](const auto&) + [w = weak_from_this(), task_config, options, idle_task](const auto&) { const auto self = w.lock(); if (!self) return; + self->_pimpl->idle_task = idle_task; + // Here we update the task planner in all the RobotContexts. // The TaskManagers rely on the parameters in the task planner for // automatic retreat. Hence, we also update them whenever the @@ -2118,7 +2352,10 @@ bool FleetUpdateHandle::set_task_planner_params( self->_pimpl->name, std::move(task_config), std::move(options)); for (const auto& t : self->_pimpl->task_managers) + { t.first->task_planner(self->_pimpl->task_planner); + t.second->set_idle_task(idle_task); + } }); return true; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 14f5d2ddc..322281326 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -36,6 +36,8 @@ std::shared_ptr Node::make( auto default_qos = rclcpp::SystemDefaultsQoS(); default_qos.keep_last(100); + auto transient_qos = rclcpp::SystemDefaultsQoS() + .reliable().keep_last(100).transient_local(); node->_door_state_obs = node->create_observable( @@ -55,7 +57,7 @@ std::shared_ptr Node::make( node->_lift_request_pub = node->create_publisher( - AdapterLiftRequestTopicName, default_qos); + AdapterLiftRequestTopicName, transient_qos); node->_task_summary_pub = node->create_publisher( @@ -106,6 +108,18 @@ std::shared_ptr Node::make( node->create_publisher( TaskApiResponses, transient_local_qos); + node->_mutex_group_request_pub = + node->create_publisher( + MutexGroupRequestTopicName, transient_local_qos); + + node->_mutex_group_request_obs = + node->create_observable( + MutexGroupRequestTopicName, transient_local_qos); + + node->_mutex_group_states_obs = + node->create_observable( + MutexGroupStatesTopicName, transient_local_qos); + return node; } @@ -229,5 +243,23 @@ auto Node::task_api_response() const -> const ApiResponsePub& return _task_api_response_pub; } +//============================================================================== +auto Node::mutex_group_request() const -> const MutexGroupRequestPub& +{ + return _mutex_group_request_pub; +} + +//============================================================================== +auto Node::mutex_group_request_obs() const -> const MutexGroupRequestObs& +{ + return _mutex_group_request_obs->observe(); +} + +//============================================================================== +auto Node::mutex_group_states() const -> const MutexGroupStatesObs& +{ + return _mutex_group_states_obs->observe(); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 0ced9dad7..5331b43e2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -35,6 +35,8 @@ #include #include +#include +#include #include #include @@ -122,6 +124,17 @@ class Node : public rmf_rxcpp::Transport using ApiResponsePub = rclcpp::Publisher::SharedPtr; const ApiResponsePub& task_api_response() const; + using MutexGroupRequest = rmf_fleet_msgs::msg::MutexGroupRequest; + using MutexGroupRequestPub = rclcpp::Publisher::SharedPtr; + const MutexGroupRequestPub& mutex_group_request() const; + + using MutexGroupRequestObs = rxcpp::observable; + const MutexGroupRequestObs& mutex_group_request_obs() const; + + using MutexGroupStates = rmf_fleet_msgs::msg::MutexGroupStates; + using MutexGroupStatesObs = rxcpp::observable; + const MutexGroupStatesObs& mutex_group_states() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( std::chrono::duration period, @@ -179,6 +192,9 @@ class Node : public rmf_rxcpp::Transport FleetStatePub _fleet_state_pub; Bridge _task_api_request_obs; ApiResponsePub _task_api_response_pub; + MutexGroupRequestPub _mutex_group_request_pub; + Bridge _mutex_group_request_obs; + Bridge _mutex_group_states_obs; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index efc92a5d2..0abfd7165 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -22,10 +22,325 @@ #include #include +#include + +#include namespace rmf_fleet_adapter { namespace agv { +//============================================================================== +void NavParams::search_for_location( + const std::string& map, + Eigen::Vector3d position, + RobotContext& context) +{ + auto planner = context.planner(); + if (!planner) + { + RCLCPP_ERROR( + context.node()->get_logger(), + "Planner unavailable for robot [%s], cannot update its location", + context.requester_id().c_str()); + return; + } + const auto& graph = planner->get_configuration().graph(); + const auto now = context.now(); + auto starts = compute_plan_starts(graph, map, position, now); + if (!starts.empty()) + { + if (context.debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, graph); + std::cout << ss.str() << std::endl; + } + context.set_location(std::move(starts)); + } + else + { + if (context.debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map << " <" << position.block<2, 1>(0, 0).transpose() + << "> orientation " << position[2] * 180.0 / M_PI << std::endl; + } + context.set_lost(Location { now, map, position }); + } +} + +//============================================================================== +void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) +{ + for (std::size_t i = 0; i < graph.num_waypoints() - 1; ++i) + { + const auto& wp_i = graph.get_waypoint(i); + const Eigen::Vector2d p_i = wp_i.get_location(); + const std::string& map_i = wp_i.get_map_name(); + for (std::size_t j = i+1; j < graph.num_waypoints(); ++j) + { + const auto& wp_j = graph.get_waypoint(j); + const Eigen::Vector2d p_j = wp_j.get_location(); + const std::string& map_j = wp_j.get_map_name(); + if (map_i != map_j) + { + continue; + } + + const double dist = (p_i - p_j).norm(); + if (dist > max_merge_waypoint_distance) + { + continue; + } + + // stack these waypoints + auto stack_i = stacked_vertices[i]; + auto stack_j = stacked_vertices[j]; + if (!stack_i && !stack_j) + { + // create a new stack + stack_i = std::make_shared>(); + stack_j = stack_i; + } + else if (stack_i && stack_j) + { + if (stack_i != stack_j) + { + for (const std::size_t other : *stack_j) + { + stack_i->insert(other); + stacked_vertices[other] = stack_i; + } + + } + } + else if (!stack_i) + { + stack_i = stack_j; + } + + assert(stack_i); + stack_i->insert(i); + stack_i->insert(j); + stacked_vertices[i] = stack_i; + stacked_vertices[j] = stack_j; + } + } +} + +//============================================================================== +std::string NavParams::get_vertex_name( + const rmf_traffic::agv::Graph& graph, + const std::optional v_opt) const +{ + if (!v_opt.has_value()) + return ""; + + const std::size_t v = v_opt.value(); + if (const std::string* n = graph.get_waypoint(v).name()) + { + return *n; + } + else + { + const auto& stack_it = stacked_vertices.find(v); + if (stack_it != stacked_vertices.end() && stack_it->second) + { + for (const auto& v : *stack_it->second) + { + if (const auto* n = graph.get_waypoint(v).name()) + { + return *n; + } + } + } + } + + return ""; +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::process_locations( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + return _lift_boundary_filter(graph, _descend_stacks(graph, locations)); +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + std::vector remove; + for (std::size_t i = 0; i < locations.size(); ++i) + { + rmf_traffic::agv::Plan::Start& location = locations[i]; + std::optional waypoint_opt; + if (location.lane().has_value()) + { + const rmf_traffic::agv::Graph::Lane& lane = + graph.get_lane(*location.lane()); + waypoint_opt = lane.entry().waypoint_index(); + } + else + { + waypoint_opt = location.waypoint(); + } + + if (!waypoint_opt.has_value()) + continue; + + const std::size_t original_waypoint = waypoint_opt.value(); + std::size_t waypoint = original_waypoint; + const auto s_it = stacked_vertices.find(waypoint); + VertexStack stack; + if (s_it != stacked_vertices.end()) + stack = s_it->second; + if (!stack) + continue; + + std::unordered_set visited; + bool can_descend = true; + bool has_loop = false; + while (can_descend) + { + can_descend = false; + if (!visited.insert(waypoint).second) + { + // These stacked vertices have a loop so there's no way to find a bottom + // for it. We need to just exit here. + has_loop = true; + break; + } + + for (std::size_t v : *stack) + { + if (graph.lane_from(v, waypoint)) + { + waypoint = v; + can_descend = true; + break; + } + } + } + + if (has_loop) + { + continue; + } + + // Transfer the location estimate over to the waypoint that's at the bottom + // of the vertex stack. + if (waypoint != original_waypoint) + { + bool can_merge = true; + if (const auto r_merge = graph.get_waypoint(waypoint).merge_radius()) + { + if (const auto p_opt = location.location()) + { + const auto p = p_opt.value(); + const auto p_wp = graph.get_waypoint(waypoint).get_location(); + if ((p - p_wp).norm() > r_merge) + { + can_merge = false; + remove.push_back(i); + } + } + } + + if (can_merge) + { + location.lane(std::nullopt); + location.waypoint(waypoint); + } + } + } + + for (auto r_it = remove.rbegin(); r_it != remove.rend(); ++r_it) + { + locations.erase(locations.begin() + *r_it); + } + + return locations; +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::_lift_boundary_filter( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + const auto r_it = std::remove_if( + locations.begin(), + locations.end(), + [&graph](const rmf_traffic::agv::Plan::Start& location) + { + if (location.lane().has_value()) + { + // If the intention is to move along a lane, then it is okay to keep + // this. If the lane is entering or exiting the lift, then it should + // have the necessary events. + // TODO(@mxgrey): Should we check and make sure that the event is + // actually present? + return false; + } + + if (!location.location().has_value()) + { + // If the robot is perfectly on some waypoint then there is no need to + // filter. + return false; + } + + const Eigen::Vector2d p = location.location().value(); + + const auto robot_inside_lift = [&]() + -> rmf_traffic::agv::Graph::LiftPropertiesPtr + { + for (const auto& lift : graph.all_known_lifts()) + { + // We assume lifts never overlap so we will return the first + // positive hit. + if (lift->is_in_lift(p)) + return lift; + } + + return nullptr; + }(); + + const auto& wp = graph.get_waypoint(location.waypoint()); + const auto lift = wp.in_lift(); + // If the robot's lift status and the waypoint's lift status don't match + // then we should filter this waypoint out. + return wp.in_lift() != robot_inside_lift; + }); + + locations.erase(r_it, locations.end()); + return locations; +} + +//============================================================================== +bool NavParams::in_same_stack( + std::size_t wp0, + std::size_t wp1) const +{ + if (wp0 == wp1) + { + return true; + } + + const auto s_it = stacked_vertices.find(wp0); + if (s_it == stacked_vertices.end()) + return false; + + const auto stack = s_it->second; + if (!stack) + return false; + + return stack->count(wp1); +} + //============================================================================== std::shared_ptr RobotContext::command() { @@ -41,25 +356,43 @@ std::shared_ptr RobotContext::make_updater() //============================================================================== Eigen::Vector3d RobotContext::position() const { - assert(!_location.empty()); - const auto& l = _location.front(); - if (l.location().has_value()) + if (!_location.empty()) { - const Eigen::Vector2d& p = *l.location(); + const auto& l = _location.front(); + if (l.location().has_value()) + { + const Eigen::Vector2d& p = *l.location(); + return {p[0], p[1], l.orientation()}; + } + + const Eigen::Vector2d& p = + navigation_graph().get_waypoint(l.waypoint()).get_location(); return {p[0], p[1], l.orientation()}; } + else if (_lost.has_value() && _lost->location.has_value()) + { + return _lost->location->position; + } - const Eigen::Vector2d& p = - navigation_graph().get_waypoint(l.waypoint()).get_location(); - return {p[0], p[1], l.orientation()}; + throw std::runtime_error( + "No location information is available for [" + requester_id() + "]"); } //============================================================================== const std::string& RobotContext::map() const { - assert(!_location.empty()); - return navigation_graph() - .get_waypoint(_location.front().waypoint()).get_map_name(); + if (!_location.empty()) + { + return navigation_graph() + .get_waypoint(_location.front().waypoint()).get_map_name(); + } + else if (_lost.has_value() && _lost->location.has_value()) + { + return _lost->location->map; + } + + throw std::runtime_error( + "No location information is available for [" + requester_id() + "]"); } //============================================================================== @@ -83,10 +416,21 @@ const rmf_traffic::agv::Plan::StartSet& RobotContext::location() const //============================================================================== void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) { + for (auto& location : location_) + { + location.orientation(rmf_utils::wrap_to_pi(location.orientation())); + } + _location = std::move(location_); filter_closed_lanes(); + if (_location.empty()) { + if (debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST" << + std::endl; + } set_lost(std::nullopt); return; } @@ -142,30 +486,17 @@ void RobotContext::set_lost(std::optional location) //============================================================================== void RobotContext::filter_closed_lanes() { - if (const auto planner = *_planner) + const rmf_traffic::agv::LaneClosure* closures = get_lane_closures(); + if (closures) { - const auto& closures = planner->get_configuration().lane_closures(); for (std::size_t i = 0; i < _location.size(); ) { if (_location[i].lane().has_value()) { - if (closures.is_closed(*_location[i].lane())) + if (closures->is_closed(*_location[i].lane())) { - if (_location.size() > 1) - { - _location.erase(_location.begin() + i); - continue; - } - else - { - RCLCPP_WARN( - node()->get_logger(), - "Robot [%s] is being forced to use closed lane [%lu] because it " - "has not been provided any other feasible lanes to use.", - requester_id().c_str(), - *_location[i].lane()); - return; - } + _location.erase(_location.begin() + i); + continue; } } ++i; @@ -173,6 +504,27 @@ void RobotContext::filter_closed_lanes() } } +//============================================================================== +const rmf_traffic::agv::LaneClosure* RobotContext::get_lane_closures() const +{ + if (_emergency) + { + if (const auto planner = *_emergency_planner) + { + return &planner->get_configuration().lane_closures(); + } + } + else + { + if (const auto planner = *_planner) + { + return &planner->get_configuration().lane_closures(); + } + } + + return nullptr; +} + //============================================================================== rmf_traffic::schedule::Participant& RobotContext::itinerary() { @@ -222,6 +574,12 @@ const std::string& RobotContext::requester_id() const return _requester_id; } +//============================================================================== +rmf_traffic::ParticipantId RobotContext::participant_id() const +{ + return _itinerary.id(); +} + //============================================================================== const rmf_traffic::agv::Graph& RobotContext::navigation_graph() const { @@ -235,6 +593,13 @@ RobotContext::planner() const return *_planner; } +//============================================================================== +const std::shared_ptr& +RobotContext::emergency_planner() const +{ + return *_emergency_planner; +} + //============================================================================== std::shared_ptr RobotContext::nav_params() const { @@ -306,6 +671,13 @@ RobotContext::observe_replan_request() const return _replan_obs; } +//============================================================================== +const rxcpp::observable& +RobotContext::observe_charging_change() const +{ + return _charging_change_obs; +} + //============================================================================== void RobotContext::request_replan() { @@ -400,7 +772,7 @@ std::function RobotContext::make_get_state() rmf_task::State state; state.load_basic( self->_most_recent_valid_location.front(), - self->_charger_wp, + self->_charging_wp, self->_current_battery_soc); state.insert(GetContext{self->shared_from_this()}); @@ -420,10 +792,22 @@ const std::string* RobotContext::current_task_id() const //============================================================================== RobotContext& RobotContext::current_task_id(std::optional id) { + std::unique_lock lock(*_current_task_id_mutex); _current_task_id = std::move(id); + _initial_time_idle_outside_lift = std::nullopt; return *this; } +//============================================================================== +std::string RobotContext::copy_current_task_id() const +{ + std::unique_lock lock(*_current_task_id_mutex); + if (_current_task_id.has_value()) + return _current_task_id.value(); + + return {}; +} + //============================================================================== double RobotContext::current_battery_soc() const { @@ -450,9 +834,15 @@ RobotContext& RobotContext::current_battery_soc(const double battery_soc) } //============================================================================== -std::size_t RobotContext::dedicated_charger_wp() const +std::size_t RobotContext::dedicated_charging_wp() const { - return _charger_wp; + return _charging_wp; +} + +//============================================================================== +bool RobotContext::waiting_for_charger() const +{ + return _waiting_for_charger; } //============================================================================== @@ -600,13 +990,118 @@ const Reporting& RobotContext::reporting() const return _reporting; } +//============================================================================== +bool RobotContext::localize( + EasyFullControl::Destination estimate, + EasyFullControl::CommandExecution execution) const +{ + if (_localize) + { + _localize(std::move(estimate), std::move(execution)); + return true; + } + + return false; +} + +//============================================================================== +void RobotContext::set_localization( + EasyFullControl::LocalizationRequest localization) +{ + _localize = std::move(localization); +} + +//============================================================================== +const LiftDestination* RobotContext::current_lift_destination() const +{ + return _lift_destination.get(); +} + +//============================================================================== +std::shared_ptr RobotContext::set_lift_destination( + std::string lift_name, + std::string destination_floor, + bool requested_from_inside) +{ + _lift_arrived = false; + _lift_destination = std::make_shared( + LiftDestination{ + std::move(lift_name), + std::move(destination_floor), + requested_from_inside + }); + _initial_time_idle_outside_lift = std::nullopt; + + _publish_lift_destination(); + return _lift_destination; +} + +//============================================================================== +void RobotContext::release_lift() +{ + if (_lift_destination) + { + RCLCPP_INFO( + _node->get_logger(), + "Releasing lift [%s] for [%s]", + _lift_destination->lift_name.c_str(), + requester_id().c_str()); + } + _lift_destination = nullptr; + _initial_time_idle_outside_lift = std::nullopt; + _lift_stubbornness = nullptr; + _lift_arrived = false; +} + +//============================================================================== +const std::optional& RobotContext::holding_door() const +{ + return _holding_door; +} + +//============================================================================== +const std::unordered_map& +RobotContext::locked_mutex_groups() const +{ + return _locked_mutex_groups; +} + +//============================================================================== +const rxcpp::observable& RobotContext::request_mutex_groups( + std::unordered_set groups, + rmf_traffic::Time claim_time) +{ + const auto t = rmf_traffic_ros2::convert(claim_time); + for (const auto& group : groups) + { + const auto [it, inserted] = _requesting_mutex_groups.insert({group, t}); + if (!inserted) + { + if (t.nanosec < it->second.nanosec) + it->second = t; + } + } + + _publish_mutex_group_requests(); + return _mutex_group_lock_obs; +} + +//============================================================================== +void RobotContext::retain_mutex_groups( + const std::unordered_set& retain) +{ + _retain_mutex_groups(retain, _requesting_mutex_groups); + _retain_mutex_groups(retain, _locked_mutex_groups); +} + //============================================================================== RobotContext::RobotContext( std::shared_ptr command_handle, std::vector _initial_location, rmf_traffic::schedule::Participant itinerary, std::shared_ptr schedule, - std::shared_ptr> planner, + SharedPlanner planner, + SharedPlanner emergency_planner, rmf_task::ConstActivatorPtr activator, rmf_task::ConstParametersPtr parameters, std::shared_ptr node, @@ -619,6 +1114,7 @@ RobotContext::RobotContext( _itinerary(std::move(itinerary)), _schedule(std::move(schedule)), _planner(std::move(planner)), + _emergency_planner(std::move(emergency_planner)), _task_activator(std::move(activator)), _task_parameters(std::move(parameters)), _stubbornness(std::make_shared(0)), @@ -627,7 +1123,7 @@ RobotContext::RobotContext( _maximum_delay(maximum_delay), _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), - _charger_wp(state.dedicated_charging_waypoint().value()), + _charging_wp(state.dedicated_charging_waypoint().value()), _current_task_end_state(state), _current_task_id(std::nullopt), _task_planner(std::move(task_planner)), @@ -639,8 +1135,9 @@ RobotContext::RobotContext( _replan_obs = _replan_publisher.get_observable(); _graph_change_obs = _graph_change_publisher.get_observable(); - + _charging_change_obs = _charging_change_publisher.get_observable(); _battery_soc_obs = _battery_soc_publisher.get_observable(); + _mutex_group_lock_obs = _mutex_group_lock_subject.get_observable(); _current_mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE; _override_status = std::nullopt; @@ -648,6 +1145,96 @@ RobotContext::RobotContext( _action_executor = nullptr; } +//============================================================================== +void RobotContext::schedule_itinerary( + std::shared_ptr plan_id, + rmf_traffic::schedule::Itinerary new_itinerary) +{ + bool scheduled = false; + std::size_t attempts = 0; + while (!scheduled) + { + if (++attempts > 5) + { + std::stringstream ss_sizes; + for (const auto& r : new_itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + + RCLCPP_ERROR( + node()->get_logger(), + "Repeatedly failled attempts to update schedule with an itinerary " + "containing [%lu] routes with sizes %s during LockMutexGroup " + "action for robot [%s]. Last attempted value was [%lu]. We will " + "continue without updating the traffic schedule. This could lead to " + "traffic management problems. Please report this bug to the " + "maintainers of RMF.", + new_itinerary.size(), + ss_sizes.str().c_str(), + requester_id().c_str(), + *plan_id); + break; + } + + scheduled = itinerary().set(*plan_id, new_itinerary); + + if (!scheduled) + { + for (const auto& r : new_itinerary) + { + if (r.trajectory().size() < 2) + { + std::stringstream ss_sizes; + for (const auto& r : new_itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + + RCLCPP_ERROR( + node()->get_logger(), + "Attempting to schedule an itinerary for robot [%s] containing a " + "route that has fewer than 2 waypoints. Routes sizes are %s. " + "We will continue without updating the traffic schedule. This " + "could lead to traffic management problems. Please report this " + "bug to the maintainers of RMF.", + requester_id().c_str(), + ss_sizes.str().c_str()); + return; + } + } + + *plan_id = itinerary().assign_plan_id(); + if (attempts > 1) + { + RCLCPP_ERROR( + node()->get_logger(), + "Invalid plan_id [%lu] when current plan_id is [%lu] for robot [%s] " + "while performing a LockMutexGroup. Please report this bug to an RMF " + "developer.", + *plan_id, + itinerary().current_plan_id(), + requester_id().c_str()); + } + } + } +} + +//============================================================================== +void RobotContext::schedule_hold( + std::shared_ptr plan_id, + rmf_traffic::Time time, + rmf_traffic::Duration wait, + Eigen::Vector3d position, + const std::string& map) +{ + rmf_traffic::Trajectory hold; + const auto zero = Eigen::Vector3d::Zero(); + hold.insert(time, position, zero); + hold.insert(time + wait, position, zero); + schedule_itinerary(plan_id, {rmf_traffic::Route(map, std::move(hold))}); +} + //============================================================================== void RobotContext::_set_task_manager(std::shared_ptr mgr) { @@ -660,5 +1247,351 @@ void RobotContext::_set_negotiation_license(std::shared_ptr license) _negotiation_license = std::move(license); } +//============================================================================== +void RobotContext::_set_emergency(bool value) +{ + _emergency = value; + if (_emergency) + { + filter_closed_lanes(); + } +} + +//============================================================================== +void RobotContext::_set_charging(std::size_t wp, bool waiting_for_charger) +{ + _charging_wp = wp; + _waiting_for_charger = waiting_for_charger; + _charging_change_publisher.get_subscriber().on_next(Empty{}); +} + +//============================================================================== +void RobotContext::_hold_door(std::string door_name) +{ + _holding_door = std::move(door_name); +} + +//============================================================================== +void RobotContext::_release_door(const std::string& door_name) +{ + if (_holding_door.has_value() && *_holding_door == door_name) + _holding_door = std::nullopt; +} + +//============================================================================== +void RobotContext::_check_lift_state( + const rmf_lift_msgs::msg::LiftState& state) +{ + if (_lift_destination.use_count() < 2) + { + if (_lift_destination && !_lift_destination->requested_from_inside) + { + // The lift destination reference count dropping to one while the lift + // destination request is on the outside means the task that prompted the + // lift usage was cancelled while the robot was outside of the lift. + // Therefore we should release the usage of the lift. + release_lift(); + } + else if (_lift_destination && !_current_task_id.has_value()) + { + const Eigen::Vector2d p = position().block<2, 1>(0, 0); + const auto& graph = navigation_graph(); + const auto found_lift = + graph.find_known_lift(_lift_destination->lift_name); + + bool inside_lift = false; + if (found_lift) + { + inside_lift = found_lift->is_in_lift(p); + } + + if (inside_lift) + { + _initial_time_idle_outside_lift = std::nullopt; + } + else + { + const auto now = std::chrono::steady_clock::now(); + if (_initial_time_idle_outside_lift.has_value()) + { + const auto lapse = now - *_initial_time_idle_outside_lift; + if (lapse > std::chrono::seconds(2)) + { + RCLCPP_INFO( + _node->get_logger(), + "Releasing lift [%s] for robot [%s] because it has remained idle " + "outside of the lift.", + _lift_destination->lift_name.c_str(), + requester_id().c_str()); + } + release_lift(); + } + else + { + _initial_time_idle_outside_lift = now; + } + } + } + } + + if (state.session_id == requester_id()) + { + if (!_lift_destination || _lift_destination->lift_name != state.lift_name) + { + rmf_lift_msgs::msg::LiftRequest msg; + msg.lift_name = state.lift_name; + msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; + msg.session_id = requester_id(); + _node->lift_request()->publish(msg); + } + + if (_lift_destination && _lift_destination->lift_name == state.lift_name) + { + if (!_lift_stubbornness) + { + // Be a stubborn negotiator while using the lift + _lift_stubbornness = be_stubborn(); + } + + _lift_arrived = + _lift_destination->destination_floor == state.current_floor; + } + } + else if (_lift_destination && _lift_destination->lift_name == state.lift_name) + { + RCLCPP_INFO( + _node->get_logger(), + "[%s] is waiting to begin a session with lift [%s] but the lift is " + "currently held by [%s]", + _requester_id.c_str(), + _lift_destination->lift_name.c_str(), + state.session_id.c_str()); + } + + _publish_lift_destination(); +} + +//============================================================================== +void RobotContext::_publish_lift_destination() +{ + if (!_lift_destination) + { + return; + } + + rmf_lift_msgs::msg::LiftRequest msg; + msg.lift_name = _lift_destination->lift_name; + msg.destination_floor = _lift_destination->destination_floor; + msg.session_id = requester_id(); + msg.request_time = _node->now(); + msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_AGV_MODE; + msg.door_state = rmf_lift_msgs::msg::LiftRequest::DOOR_OPEN; + + _node->lift_request()->publish(msg); +} + +//============================================================================== +void RobotContext::_check_door_supervisor( + const rmf_door_msgs::msg::SupervisorHeartbeat& state) +{ + const auto now = std::chrono::steady_clock::now(); + const auto dt = std::chrono::seconds(10); + if (_last_active_task_time + dt < now) + { + // Do not hold a door if a robot is idle for more than 10 seconds + _holding_door = std::nullopt; + } + + for (const auto& door : state.all_sessions) + { + for (const auto& session : door.sessions) + { + if (session.requester_id == _requester_id) + { + if (!_holding_door.has_value() || *_holding_door != door.door_name) + { + // We should not be holding this door open + _node->door_request()->publish( + rmf_door_msgs::build() + .request_time(_node->now()) + .requester_id(_requester_id) + .door_name(door.door_name) + .requested_mode( + rmf_door_msgs::build() + .value(rmf_door_msgs::msg::DoorMode::MODE_CLOSED))); + } + } + } + } +} + +//============================================================================== +void RobotContext::_check_mutex_groups( + const rmf_fleet_msgs::msg::MutexGroupStates& states) +{ + // Make sure to release any mutex groups that this robot is not trying to + // lock right now. + for (const auto& assignment : states.assignments) + { + if (assignment.claimant != participant_id()) + { + if (_requesting_mutex_groups.count(assignment.group) > 0) + { + if (assignment.claimant == (uint64_t)(-1)) + { + RCLCPP_INFO( + _node->get_logger(), + "[%s] is waiting for the mutex superviser to receive its request " + "for mutex group [%s]", + _requester_id.c_str(), + assignment.group.c_str()); + } + else + { + std::string holder; + if (const auto p = _schedule->get_participant(assignment.claimant)) + { + holder = p->owner() + "/" + p->name(); + } + else + { + holder = "unknown participant #" + + std::to_string(assignment.claimant); + } + + RCLCPP_INFO( + _node->get_logger(), + "[%s] is waiting to lock mutex group [%s] but that mutex is " + "currently held by [%s]", + _requester_id.c_str(), + assignment.group.c_str(), + holder.c_str()); + } + } + + continue; + } + + if (_requesting_mutex_groups.count(assignment.group) > 0) + { + _requesting_mutex_groups.erase(assignment.group); + _locked_mutex_groups[assignment.group] = assignment.claim_time; + _mutex_group_lock_subject.get_subscriber().on_next(assignment.group); + } + else if (_locked_mutex_groups.count(assignment.group) == 0) + { + // This assignment does not match either the requested nor any currently + // locked mutex groups. This is an error so let's release it. + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(assignment.group) + .claimant(participant_id()) + .claim_time(assignment.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + } + } +} + +void RobotContext::_retain_mutex_groups( + const std::unordered_set& retain, + std::unordered_map& groups) +{ + std::vector release; + for (const auto& [name, time] : groups) + { + if (retain.count(name) == 0) + { + release.push_back(MutexGroupData{name, time}); + } + } + + for (const auto& data : release) + { + _release_mutex_group(data); + _locked_mutex_groups.erase(data.name); + } +} + +//============================================================================== +void RobotContext::_release_mutex_group(const MutexGroupData& data) const +{ + if (data.name.empty()) + { + return; + } + + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(data.name) + .claimant(participant_id()) + .claim_time(data.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); +} + +//============================================================================== +void RobotContext::_publish_mutex_group_requests() +{ + const auto now = std::chrono::steady_clock::now(); + if (_current_task_id.has_value()) + { + _last_active_task_time = now; + } + else + { + if (_last_active_task_time + std::chrono::seconds(10) < now) + { + // The robot has been idle for 10 seconds. It should not be keeping a + // mutex locked; a task probably ended wrongly. + if (!_requesting_mutex_groups.empty() + || !_locked_mutex_groups.empty()) + { + auto warning = [&](const std::string& name) + { + RCLCPP_ERROR( + _node->get_logger(), + "Forcibly releasing mutex group [%s] requested by robot [%s] " + "because the robot has been idle for an excessive amount of time.", + name.c_str(), + requester_id().c_str()); + }; + + for (const auto& [name, time] : _requesting_mutex_groups) + { + warning(name); + _release_mutex_group(MutexGroupData{name, time}); + } + _requesting_mutex_groups.clear(); + + for (const auto& [name, time] : _locked_mutex_groups) + { + warning(name); + _release_mutex_group(MutexGroupData{name, time}); + } + _locked_mutex_groups.clear(); + } + } + } + + auto publish = [&](const MutexGroupData& data) + { + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(data.name) + .claimant(participant_id()) + .claim_time(data.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); + }; + + for (const auto& [name, time] : _requesting_mutex_groups) + { + publish(MutexGroupData{name, time}); + } + + for (const auto& [name, time] : _locked_mutex_groups) + { + publish(MutexGroupData{name, time}); + } +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index bafd331de..9c82be580 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,12 +37,15 @@ #include #include +#include #include #include "Node.hpp" #include "../Reporting.hpp" +#include + namespace rmf_fleet_adapter { // Forward declaration @@ -49,7 +53,191 @@ class TaskManager; namespace agv { +class RobotContext; using TransformDictionary = std::unordered_map; +using SharedPlanner = std::shared_ptr< + std::shared_ptr>; +using Destination = EasyFullControl::Destination; +using VertexStack = std::shared_ptr>; +using TimeMsg = builtin_interfaces::msg::Time; + +//============================================================================== +class EventPrinter : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + EventPrinter() + { + // Do nothing + } + + void execute(const DoorOpen& e) override + { + text = "DoorOpen " + e.name(); + } + + void execute(const DoorClose& e) override + { + text = "DoorClose " + e.name(); + } + + void execute(const LiftSessionBegin& e) override + { + text = "LiftSessionBegin " + e.lift_name(); + } + + void execute(const LiftDoorOpen& e) override + { + text = "LiftDoorOpen " + e.lift_name(); + } + + void execute(const LiftSessionEnd& e) override + { + text = "LiftSessionEnd " + e.lift_name(); + } + + void execute(const LiftMove& e) override + { + text = "LiftMove " + e.lift_name(); + } + + void execute(const Wait&) override + { + text = "Wait"; + } + + void execute(const Dock& dock) override + { + text = "Dock " + dock.dock_name(); + } + + std::string text; +}; + +//============================================================================== +inline std::string print_waypoint( + const std::size_t i_wp, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + const rmf_traffic::agv::Graph::Waypoint& wp = graph.get_waypoint(i_wp); + + ss << wp.get_map_name() << " <" << wp.get_location().transpose() << "> [" + << wp.name_or_index() << "]"; + return ss.str(); +} + +//============================================================================== +inline std::string print_plan_waypoint( + const rmf_traffic::agv::Plan::Waypoint& wp, + const rmf_traffic::agv::Graph& graph, + const rmf_traffic::Time t0 = rmf_traffic::Time(rmf_traffic::Duration(0))) +{ + std::stringstream ss; + ss << "t=" << rmf_traffic::time::to_seconds(wp.time() - t0); + if (wp.graph_index().has_value()) + ss << " #" << *wp.graph_index(); + ss << " <" << wp.position().transpose() + << "> yaw=" << wp.position()[2] * 180.0 / M_PI; + if (wp.event()) + { + EventPrinter event; + wp.event()->execute(event); + ss << " event [" << event.text << "]"; + } + + if (wp.graph_index().has_value()) + { + const auto& m = graph.get_waypoint(*wp.graph_index()).in_mutex_group(); + if (!m.empty()) + { + ss << " initial mutex [" << m << "]"; + } + } + + ss << " approach lanes:"; + for (const auto& l : wp.approach_lanes()) + { + ss << " " << l; + const auto& lane = graph.get_lane(l); + const auto& m = lane.properties().in_mutex_group(); + if (!m.empty()) + { + ss << " [" << m << "]"; + } + } + + return ss.str(); +} + +//============================================================================== +inline std::string print_lane_node( + const rmf_traffic::agv::Graph::Lane::Node& node, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + EventPrinter event; + if (node.event()) + node.event()->execute(event); + + ss << "{ " << print_waypoint(node.waypoint_index(), graph); + if (!event.text.empty()) + ss << " event " << event.text; + ss << " }"; + + return ss.str(); +} + +//============================================================================== +inline std::string print_lane( + const std::size_t i_lane, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + const auto& lane = graph.get_lane(i_lane); + ss << "lane " << i_lane << ": " << print_lane_node(lane.entry(), graph) + << " -> " << print_lane_node(lane.exit(), graph); + return ss.str(); +} + +//============================================================================== +inline std::string print_starts( + const rmf_traffic::agv::Plan::StartSet& starts, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + for (const rmf_traffic::agv::Plan::Start& l : starts) + { + ss << "\n -- "; + if (l.lane().has_value()) + { + ss << print_lane(*l.lane(), graph); + + const auto& lane = graph.get_lane(*l.lane()); + if (l.waypoint() != lane.exit().waypoint_index()) + { + ss << " !! MISMATCH BETWEEN KEY WAYPOINT AND LANE EXIT: key " + << l.waypoint() << " vs exit " << lane.exit().waypoint_index(); + } + } + else + { + ss << print_waypoint(l.waypoint(), graph); + } + + if (l.location().has_value()) + { + ss << " | location <" << l.location()->transpose() << ">"; + } + else + { + ss << " | on waypoint"; + } + + ss << " | orientation " << l.orientation() * 180.0 / M_PI; + } + + return ss.str(); +} //============================================================================== struct NavParams @@ -81,6 +269,27 @@ struct NavParams return tf_it->second.apply_inverse(position); } + std::optional to_robot_coordinates( + const std::string& map, + Eigen::Vector3d position) + { + if (!transforms_to_robot_coords) + return position; + + const auto tf_it = transforms_to_robot_coords->find(map); + if (tf_it == transforms_to_robot_coords->end()) + { + return std::nullopt; + } + + return tf_it->second.apply(position); + } + + void search_for_location( + const std::string& map, + Eigen::Vector3d position, + RobotContext& context); + rmf_traffic::agv::Plan::StartSet compute_plan_starts( const rmf_traffic::agv::Graph& graph, const std::string& map_name, @@ -99,11 +308,35 @@ struct NavParams min_lane_length); if (!starts.empty()) - return starts; + return process_locations(graph, starts); } return {}; } + + std::unordered_map stacked_vertices; + + void find_stacked_vertices(const rmf_traffic::agv::Graph& graph); + + std::string get_vertex_name( + const rmf_traffic::agv::Graph& graph, + std::optional v) const; + + rmf_traffic::agv::Plan::StartSet process_locations( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; + + rmf_traffic::agv::Plan::StartSet _descend_stacks( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; + + // If one of the locations is associated with a lift vertex, filter it out if + // the actual of the robot is outside the dimensions of the lift. + rmf_traffic::agv::Plan::StartSet _lift_boundary_filter( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; + + bool in_same_stack(std::size_t wp0, std::size_t wp1) const; }; //============================================================================== @@ -114,6 +347,7 @@ struct Location Eigen::Vector3d position; }; +//============================================================================== /// Store position information when the robot is lost, i.e. it has diverged too /// far from its navigation graph. struct Lost @@ -126,6 +360,36 @@ struct Lost std::unique_ptr ticket; }; +//============================================================================== +struct LiftDestination +{ + std::string lift_name; + std::string destination_floor; + bool requested_from_inside; + + inline bool matches( + const std::string& desired_lift_name, + const std::string& desired_floor) const + { + return desired_lift_name == lift_name && desired_floor == destination_floor; + } +}; + +//============================================================================== +struct MutexGroupData +{ + std::string name; + TimeMsg claim_time; +}; + +//============================================================================== +struct MutexGroupSwitch +{ + std::string from; + std::string to; + std::function accept; +}; + //============================================================================== class RobotContext : public std::enable_shared_from_this, @@ -172,6 +436,9 @@ class RobotContext /// plan starts are using closed lanes. void filter_closed_lanes(); + /// Get the current lane closures for the robot + const rmf_traffic::agv::LaneClosure* get_lane_closures() const; + /// Get a mutable reference to the schedule of this robot rmf_traffic::schedule::Participant& itinerary(); @@ -198,12 +465,20 @@ class RobotContext /// Get the requester ID to use for this robot when sending requests const std::string& requester_id() const; + /// Get the traffic participant ID for this robot + rmf_traffic::ParticipantId participant_id() const; + /// Get the navigation graph used by this robot const rmf_traffic::agv::Graph& navigation_graph() const; - /// Get a mutable reference to the planner for this robot + /// Get an immutable reference to the planner for this robot const std::shared_ptr& planner() const; + /// Get an immutable reference to the planner to use for this robot during + /// emergencies (fire alarm). + const std::shared_ptr& emergency_planner() + const; + /// Get the navigation params for this robot, if it has any. This will only be /// available for EasyFullControl robots. std::shared_ptr nav_params() const; @@ -228,8 +503,12 @@ class RobotContext bool is_stubborn() const; struct Empty {}; + /// Use this to get notified when const rxcpp::observable& observe_replan_request() const; + /// Use this to get notified when the charging policy changes for this robot. + const rxcpp::observable& observe_charging_change() const; + /// Request this robot to replan void request_replan(); @@ -286,6 +565,10 @@ class RobotContext /// being performed. RobotContext& current_task_id(std::optional id); + /// Get a string copy of the current task ID of the robot, or an empty string + /// if the robot is not performing any task + std::string copy_current_task_id() const; + /// Get the current battery state of charge double current_battery_soc() const; @@ -293,7 +576,16 @@ class RobotContext /// publishes the battery soc via _battery_soc_publisher. RobotContext& current_battery_soc(const double battery_soc); - std::size_t dedicated_charger_wp() const; + /// The waypoint dedicated to this robot that it should go to if it needs to + /// charge. This may be a waypoint that has a charger or it may be a parking + /// spot where the robot should wait until a charger becomes available. Use + /// waiting_for_charger() to determine which. + std::size_t dedicated_charging_wp() const; + + /// When the robot reaches its dedicated_charging_wp, is it there to wait for + /// a charger to become available (true) or to actually perform the charging + /// (false)? + bool waiting_for_charger() const; // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; @@ -352,6 +644,51 @@ class RobotContext const Reporting& reporting() const; + /// Tell the robot to localize near here + bool localize( + EasyFullControl::Destination estimate, + EasyFullControl::CommandExecution execution) const; + + /// Set the callback for localizing the robot + void set_localization(EasyFullControl::LocalizationRequest localization); + + /// Get the current lift destination request for this robot + const LiftDestination* current_lift_destination() const; + + /// Ask for a certain lift to go to a certain destination and open the doors + std::shared_ptr set_lift_destination( + std::string lift_name, + std::string destination_floor, + bool requested_from_inside); + + /// Indicate that the lift is no longer needed + void release_lift(); + + /// Check if a door is being held + const std::optional& holding_door() const; + + /// What mutex group is currently being locked. + const std::unordered_map& locked_mutex_groups() const; + + /// Set the mutex group that this robot needs to lock. + const rxcpp::observable& request_mutex_groups( + std::unordered_set groups, + rmf_traffic::Time claim_time); + + /// Retain only the mutex groups listed in the set. Release all others. + void retain_mutex_groups(const std::unordered_set& groups); + + void schedule_itinerary( + std::shared_ptr plan_id, + rmf_traffic::schedule::Itinerary itinerary); + + void schedule_hold( + std::shared_ptr plan_id, + rmf_traffic::Time time, + rmf_traffic::Duration wait, + Eigen::Vector3d position, + const std::string& map); + /// Set the task manager for this robot. This should only be called in the /// TaskManager::make function. void _set_task_manager(std::shared_ptr mgr); @@ -360,12 +697,84 @@ class RobotContext /// the FleetUpdateHandle::add_robot function. void _set_negotiation_license(std::shared_ptr license); + /// Use this to trigger emergencies on/off. This should only be called in the + /// FleetUpdateHandle::handle_emergency function. + void _set_emergency(bool value); + + /// Use this to change the charging settings for the robot and trigger a + /// charger change notification. + void _set_charging(std::size_t wp, bool waiting_for_charger); + + /// Request a door to stay open. This should only be used by DoorOpen. + void _hold_door(std::string door_name); + + /// Release a door. This should only be used by DoorClose + void _release_door(const std::string& door_name); + + template + static std::shared_ptr make(Args&&... args) + { + auto context = std::shared_ptr( + new RobotContext(std::forward(args)...)); + + context->_lift_subscription = context->_node->lift_state() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_check_lift_state(*msg); + }); + + context->_door_subscription = context->_node->door_supervisor() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_check_door_supervisor(*msg); + }); + + context->_mutex_group_sanity_check = context->_node->mutex_group_states() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_check_mutex_groups(*msg); + }); + + context->_mutex_group_heartbeat = context->_node->try_create_wall_timer( + std::chrono::seconds(2), + [w = context->weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; + + self->_publish_mutex_group_requests(); + }); + + return context; + } + + bool debug_positions = false; + +private: + RobotContext( std::shared_ptr command_handle, std::vector _initial_location, rmf_traffic::schedule::Participant itinerary, std::shared_ptr schedule, - std::shared_ptr> planner, + SharedPlanner planner, + SharedPlanner emergency_planner, rmf_task::ConstActivatorPtr activator, rmf_task::ConstParametersPtr parameters, std::shared_ptr node, @@ -374,14 +783,13 @@ class RobotContext rmf_task::State state, std::shared_ptr task_planner); -private: - std::weak_ptr _command_handle; std::vector _location; std::vector _most_recent_valid_location; rmf_traffic::schedule::Participant _itinerary; std::shared_ptr _schedule; - std::shared_ptr> _planner; + SharedPlanner _planner; + SharedPlanner _emergency_planner; std::shared_ptr _nav_params; rmf_task::ConstActivatorPtr _task_activator; rmf_task::ConstParametersPtr _task_parameters; @@ -393,6 +801,9 @@ class RobotContext rxcpp::subjects::subject _replan_publisher; rxcpp::observable _replan_obs; + rxcpp::subjects::subject _charging_change_publisher; + rxcpp::observable _charging_change_obs; + rxcpp::subjects::subject _graph_change_publisher; rxcpp::observable _graph_change_obs; @@ -405,17 +816,24 @@ class RobotContext /// Always call the current_battery_soc() setter to set a new value double _current_battery_soc = 1.0; - std::size_t _charger_wp; + std::size_t _charging_wp; + /// When the robot reaches its _charging_wp, is there to wait for a charger + /// (true) or to actually charge (false)? + bool _waiting_for_charger; rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; rmf_task::State _current_task_end_state; std::optional _current_task_id; + std::unique_ptr _current_task_id_mutex = + std::make_unique(); std::shared_ptr _task_planner; std::weak_ptr _task_manager; RobotUpdateHandle::Unstable::Watchdog _lift_watchdog; rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0); bool _commissioned = true; + bool _emergency = false; + EasyFullControl::LocalizationRequest _localize; // Mode value for RobotMode message uint32_t _current_mode; @@ -425,6 +843,34 @@ class RobotContext Reporting _reporting; /// Keep track of a lost robot std::optional _lost; + + void _check_lift_state(const rmf_lift_msgs::msg::LiftState& state); + void _publish_lift_destination(); + std::shared_ptr _lift_destination; + rmf_rxcpp::subscription_guard _lift_subscription; + std::optional + _initial_time_idle_outside_lift; + std::shared_ptr _lift_stubbornness; + bool _lift_arrived = false; + + void _check_door_supervisor( + const rmf_door_msgs::msg::SupervisorHeartbeat& hb); + std::optional _holding_door; + rmf_rxcpp::subscription_guard _door_subscription; + + void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); + void _retain_mutex_groups( + const std::unordered_set& retain, + std::unordered_map& _groups); + void _release_mutex_group(const MutexGroupData& data) const; + void _publish_mutex_group_requests(); + std::unordered_map _requesting_mutex_groups; + std::unordered_map _locked_mutex_groups; + rxcpp::subjects::subject _mutex_group_lock_subject; + rxcpp::observable _mutex_group_lock_obs; + rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; + rmf_rxcpp::subscription_guard _mutex_group_sanity_check; + std::chrono::steady_clock::time_point _last_active_task_time; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 1a617ebe9..46fbb44f1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -77,11 +77,19 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, waypoint, orientation](const auto&) { - context->set_location({ + rmf_traffic::agv::Plan::StartSet starts = { rmf_traffic::agv::Plan::Start( rmf_traffic_ros2::convert(context->node()->now()), waypoint, orientation) - }); + }; + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } + context->set_location(starts); }); } } @@ -117,6 +125,13 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(starts)](const auto&) { + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } context->set_location(std::move(starts)); }); } @@ -132,11 +147,19 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, position, waypoint](const auto&) { - context->set_location({ + rmf_traffic::agv::Plan::StartSet starts = { rmf_traffic::agv::Plan::Start( rmf_traffic_ros2::convert(context->node()->now()), waypoint, position[2], Eigen::Vector2d(position.block<2, 1>(0, 0))) - }); + }; + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } + context->set_location(std::move(starts)); }); } } @@ -167,8 +190,16 @@ void RobotUpdateHandle::update_position( position[0], position[1], position[2], map_name.c_str()); context->worker().schedule( - [context, now, map_name, position](const auto&) + [context, now, map_name, position]( + const auto&) { + if (context->debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map_name << " <" << position.block<2, 1>(0, + 0).transpose() + << "> orientation " << position[2] * 180.0 / M_PI << std::endl; + } context->set_lost(Location { now, map_name, position }); }); return; @@ -177,6 +208,13 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(starts)](const auto&) { + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } context->set_location(std::move(starts)); }); } @@ -191,6 +229,13 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(position)](const auto&) { + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } context->set_location(starts); }); } @@ -202,14 +247,20 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( { if (const auto context = _pimpl->get_context()) { - auto end_state = context->current_task_end_state(); - end_state.dedicated_charging_waypoint(charger_wp); - context->current_task_end_state(end_state); - RCLCPP_INFO( - context->node()->get_logger(), - "Charger waypoint for robot [%s] set to index [%ld]", - context->name().c_str(), - charger_wp); + context->worker().schedule([charger_wp, w = context->weak_from_this()]( + const auto&) + { + const auto self = w.lock(); + if (!self) + return; + + self->_set_charging(charger_wp, true); + RCLCPP_INFO( + self->node()->get_logger(), + "Charger waypoint for robot [%s] set to index [%ld]", + self->requester_id().c_str(), + charger_wp); + }); } return *this; @@ -331,6 +382,15 @@ RobotUpdateHandle::maximum_delay() const return rmf_utils::nullopt; } +//============================================================================== +const std::string RobotUpdateHandle::current_task_id() const +{ + if (const auto context = _pimpl->get_context()) + return context->copy_current_task_id(); + + return {}; +} + //============================================================================== void RobotUpdateHandle::set_action_executor( RobotUpdateHandle::ActionExecutor action_executor) @@ -638,6 +698,28 @@ void RobotUpdateHandle::enable_responsive_wait(bool value) }); } +//============================================================================== +void RobotUpdateHandle::release_lift() +{ + const auto context = _pimpl->get_context(); + if (!context) + return; + + context->worker().schedule( + [context](const auto&) + { + if (const auto* lift = context->current_lift_destination()) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Releasing lift [%s] for [%s] because of a user request", + lift->lift_name.c_str(), + context->requester_id().c_str()); + } + context->release_lift(); + }); +} + //============================================================================== RobotUpdateHandle::RobotUpdateHandle() { @@ -813,6 +895,17 @@ void RobotUpdateHandle::Unstable::set_lift_entry_watchdog( } } +//============================================================================== +void RobotUpdateHandle::Unstable::debug_positions(bool on) +{ + if (const auto context = _pimpl->get_context()) + { + // No need to worry about race conditions or data races here because this is + // a mostly inconsequential bool + context->debug_positions = on; + } +} + //============================================================================== void RobotUpdateHandle::ActionExecution::update_remaining_time( rmf_traffic::Duration remaining_time_estimate) @@ -1090,26 +1183,12 @@ void ScheduleOverride::overridden_update( } } - auto planner = context->planner(); - if (!planner) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context->requester_id().c_str()); - return; - } - - const auto& graph = planner->get_configuration().graph(); - auto starts = nav_params->compute_plan_starts(graph, map, location, now); - if (!starts.empty()) - { - context->set_location(std::move(starts)); - } - else + if (context->debug_positions) { - context->set_lost(Location { now, map, location }); + std::cout << "Search for location from " << __FILE__ << "|" << __LINE__ << + std::endl; } + nav_params->search_for_location(map, location, *context); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp index 80f77221d..ad8b2ea90 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp @@ -99,7 +99,7 @@ Eigen::Vector3d Transformation::apply( Eigen::Vector3d Transformation::apply_inverse( const Eigen::Vector3d& position) const { - Eigen::Vector2d p = _pimpl->transform * position.block<2, 1>(0, 0); + Eigen::Vector2d p = _pimpl->transform_inv * position.block<2, 1>(0, 0); double angle = rmf_utils::wrap_to_pi(position[2] - _pimpl->rotation); return Eigen::Vector3d(p[0], p[1], angle); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp index 6d3c9c896..f56291c12 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp @@ -22,6 +22,8 @@ namespace rmf_fleet_adapter { namespace agv { +class RobotContext; + class EasyCommandHandle; using EasyCommandHandlePtr = std::shared_ptr; @@ -67,5 +69,68 @@ class EasyFullControl::Implementation } }; +//============================================================================== +class EasyFullControl::Destination::Implementation +{ +public: + std::string map; + Eigen::Vector3d position; + std::optional graph_index; + std::string name; + std::optional speed_limit; + rmf_traffic::agv::Graph::LiftPropertiesPtr lift; + std::optional dock = std::nullopt; + + template + static Destination make(Args&&... args) + { + Destination output; + output._pimpl = rmf_utils::make_impl( + Implementation{std::forward(args)...}); + return output; + } + + static Implementation& get(Destination& self) + { + return *self._pimpl; + } +}; + +class EasyFullControl::CommandExecution::Implementation +{ +public: + struct Data; + using DataPtr = std::shared_ptr; + + std::weak_ptr w_context; + std::shared_ptr data; + std::function begin; + std::function finisher; + ActivityIdentifierPtr identifier; + + void finish(); + + Stubbornness override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold); + + static CommandExecution make( + const std::shared_ptr& context, + Data data_, + std::function begin); + + static CommandExecution make_hold( + const std::shared_ptr& context, + rmf_traffic::Time expected_time, + rmf_traffic::PlanId plan_id, + std::function finisher); + + static Implementation& get(CommandExecution& cmd) + { + return *cmd._pimpl; + } +}; + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 17bc2c2b1..22369f3ff 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -232,6 +233,18 @@ class ParticipantFactoryRos2 : public ParticipantFactory rmf_traffic_ros2::schedule::WriterPtr _writer; }; +//============================================================================== +struct Expectations +{ + std::vector states; + std::vector pending_requests; +}; + +//============================================================================== +// Map task id to pair of +using TaskAssignments = rmf_task::TaskPlanner::Assignments; +class AllocateTasks; + //============================================================================== class FleetUpdateHandle::Implementation { @@ -239,7 +252,7 @@ class FleetUpdateHandle::Implementation std::weak_ptr weak_self; std::string name; - std::shared_ptr> planner; + SharedPlanner planner; std::shared_ptr node; rxcpp::schedulers::worker worker; std::shared_ptr writer; @@ -259,6 +272,7 @@ class FleetUpdateHandle::Implementation rmf_task::BinaryPriorityScheme::make_cost_calculator(); std::shared_ptr task_parameters = nullptr; std::shared_ptr task_planner = nullptr; + rmf_task::ConstRequestFactoryPtr idle_task = nullptr; rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); @@ -277,8 +291,23 @@ class FleetUpdateHandle::Implementation rclcpp::TimerBase::SharedPtr fleet_state_update_timer = nullptr; rclcpp::TimerBase::SharedPtr memory_trim_timer = nullptr; - // Map task id to pair of - using Assignments = rmf_task::TaskPlanner::Assignments; + rxcpp::subscription emergency_sub; + rxcpp::subjects::subject emergency_publisher; + rxcpp::observable emergency_obs; + bool emergency_active = false; + // When an emergency (fire alarm) is active, this map says which level each + // lift will "home" to (if any). + std::unordered_map emergency_level_for_lift; + SharedPlanner emergency_planner; + + rclcpp::Subscription::SharedPtr + charging_assignments_sub = nullptr; + using ChargingAssignments = rmf_fleet_msgs::msg::ChargingAssignments; + using ChargingAssignment = rmf_fleet_msgs::msg::ChargingAssignment; + // Keep track of charging assignments for robots that have not been registered + // yet. + std::unordered_map + unregistered_charging_assignments; using DockParamMap = std::unordered_map< @@ -299,7 +328,7 @@ class FleetUpdateHandle::Implementation double current_assignment_cost = 0.0; // Map to store task id with assignments for BidNotice - std::unordered_map bid_notice_assignments = {}; + std::unordered_map bid_notice_assignments = {}; using BidNoticeMsg = rmf_task_msgs::msg::BidNotice; @@ -325,6 +354,9 @@ class FleetUpdateHandle::Implementation std::unordered_map speed_limited_lanes = {}; std::unordered_set closed_lanes = {}; + std::shared_ptr calculate_bid; + rmf_rxcpp::subscription_guard calculate_bid_subscription; + template static std::shared_ptr make(Args&&... args) { @@ -334,6 +366,21 @@ class FleetUpdateHandle::Implementation handle->_pimpl->add_standard_tasks(); + handle->_pimpl->emergency_obs = + handle->_pimpl->emergency_publisher.get_observable(); + handle->_pimpl->emergency_sub = handle->_pimpl->node->emergency_notice() + .observe_on(rxcpp::identity_same_worker(handle->_pimpl->worker)) + .subscribe( + [w = handle->weak_from_this()](const auto& msg) + { + if (const auto self = w.lock()) + { + self->_pimpl->handle_emergency(msg->data); + } + }); + handle->_pimpl->emergency_planner = + std::make_shared>(nullptr); + // TODO(MXG): This is a very crude implementation. We create a dummy set of // task planner parameters to stand in until the user sets the task planner // parameters. We'll distribute this shared_ptr to the robot contexts and @@ -533,6 +580,17 @@ class FleetUpdateHandle::Implementation } }; + handle->_pimpl->charging_assignments_sub = + handle->_pimpl->node->create_subscription< + rmf_fleet_msgs::msg::ChargingAssignments>( + ChargingAssignmentsTopicName, + reliable_transient_qos, + [w = handle->weak_from_this()](const ChargingAssignments& assignments) + { + if (const auto self = w.lock()) + self->_pimpl->update_charging_assignments(assignments); + }); + handle->_pimpl->deserialization.event->add( "perform_action", validator, deserializer); @@ -562,25 +620,11 @@ class FleetUpdateHandle::Implementation std::optional get_nearest_charger( const rmf_traffic::agv::Planner::Start& start); - struct Expectations - { - std::vector states; - std::vector pending_requests; - }; - Expectations aggregate_expectations() const; - /// Generate task assignments for a collection of task requests comprising of - /// task requests currently in TaskManager queues while optionally including a - /// new request and while optionally ignoring a specific request. - std::optional allocate_tasks( - rmf_task::ConstRequestPtr new_request = nullptr, - std::vector* errors = nullptr, - std::optional expectations = std::nullopt) const; - /// Helper function to check if assignments are valid. An assignment set is /// invalid if one of the assignments has already begun execution. - bool is_valid_assignments(Assignments& assignments) const; + bool is_valid_assignments(TaskAssignments& assignments) const; void publish_fleet_state_topic() const; @@ -590,21 +634,34 @@ class FleetUpdateHandle::Implementation void update_fleet_state() const; void update_fleet_logs() const; + void handle_emergency(bool is_emergency); + void update_emergency_planner(); + + void update_charging_assignments(const ChargingAssignments& assignments); nlohmann::json_schema::json_validator make_validator( const nlohmann::json& schema) const; void add_standard_tasks(); - std::string make_error_str( - uint64_t code, std::string category, std::string detail) const; - std::shared_ptr convert( const std::string& task_id, const nlohmann::json& request_msg, std::vector& errors) const; }; +//============================================================================== +inline std::string make_error_str( + uint64_t code, std::string category, std::string detail) +{ + nlohmann::json error; + error["code"] = code; + error["category"] = std::move(category); + error["detail"] = std::move(detail); + + return error.dump(); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index e8c8b24f4..b95dc85dc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -32,6 +32,7 @@ namespace rmf_fleet_adapter { namespace agv { +//============================================================================== class TriggerOnce { public: @@ -192,22 +193,12 @@ class RobotUpdateHandle::ActionExecution::Implementation if (nav_params) { - const auto& planner = context->planner(); - if (!planner) - return; - - const auto now = context->now(); - const auto& graph = planner->get_configuration().graph(); - auto starts = nav_params->compute_plan_starts( - graph, map, location, now); - if (!starts.empty()) - { - context->set_location(starts); - } - else + if (context->debug_positions) { - context->set_lost(Location { now, map, location }); + std::cout << "Searching for location from " << __FILE__ << "|" << + __LINE__ << std::endl; } + nav_params->search_for_location(map, location, *context); } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp index 9ab822af2..eca4e50b8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp @@ -19,9 +19,13 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { +using LiftPropertiesPtr = rmf_traffic::agv::Graph::LiftPropertiesPtr; + //============================================================================== rmf_traffic::agv::Graph parse_graph( const std::string& graph_file, @@ -33,6 +37,76 @@ rmf_traffic::agv::Graph parse_graph( throw std::runtime_error("Failed to load graph file [" + graph_file + "]"); } + rmf_traffic::agv::Graph graph; + bool has_lifts = false; + const YAML::Node lifts_yaml = graph_config["lifts"]; + if (!lifts_yaml) + { + std::cout << "Your navigation graph does not provide lift information. " + << + "This may cause problems with behaviors around lifts. Please consider " + << + "regenerating your navigration graph with the latest version of " + << "rmf_building_map_tools (from the rmf_traffic_editor repo)." + << std::endl; + } + else + { + has_lifts = true; + for (const auto& lift : lifts_yaml) + { + const std::string& name = lift.first.as(); + const YAML::Node& properties_yaml = lift.second; + + const YAML::Node& position_yaml = properties_yaml["position"]; + const Eigen::Vector2d location( + position_yaml[0].as(), + position_yaml[1].as()); + const double orientation = position_yaml[2].as(); + + const YAML::Node& dims_yaml = properties_yaml["dims"]; + const Eigen::Vector2d dimensions( + dims_yaml[0].as(), + dims_yaml[1].as()); + + graph.set_known_lift(rmf_traffic::agv::Graph::LiftProperties( + name, location, orientation, dimensions)); + } + } + + const YAML::Node doors_yaml = graph_config["doors"]; + if (!doors_yaml) + { + std::cout << "Your navigation graph does not provide door information. " + << + "This may cause problems with behaviors around doors. Please consider " + << + "regenerating your navigration graph with the latest version of " + << "rmf_building_map_tools (from the rmf_traffic_editor repo)." + << std::endl; + } + else + { + for (const auto& door : doors_yaml) + { + const std::string& name = door.first.as(); + const YAML::Node properties_yaml = door.second; + const YAML::Node& endpoints_yaml = properties_yaml["endpoints"]; + std::string map = properties_yaml["map"].as(); + + const YAML::Node& p0_yaml = endpoints_yaml[0]; + const auto p0 = Eigen::Vector2d( + p0_yaml[0].as(), p0_yaml[1].as()); + + const YAML::Node& p1_yaml = endpoints_yaml[1]; + const auto p1 = Eigen::Vector2d( + p1_yaml[0].as(), p1_yaml[1].as()); + + graph.set_known_door( + rmf_traffic::agv::Graph::DoorProperties(name, p0, p1, std::move(map))); + } + } + const YAML::Node levels = graph_config["levels"]; if (!levels) { @@ -56,9 +130,9 @@ rmf_traffic::agv::Graph parse_graph( using Lane = rmf_traffic::agv::Graph::Lane; using Event = Lane::Event; - rmf_traffic::agv::Graph graph; std::unordered_map> wps_of_lift; std::unordered_map lift_of_wp; + std::unordered_map stacked_vertex; std::size_t vnum = 0; // To increment lane endpoint ids for (const auto& level : levels) @@ -125,6 +199,12 @@ rmf_traffic::agv::Graph parse_graph( wp.set_charger(true); } + const YAML::Node& mutex_yaml = options["mutex"]; + if (mutex_yaml) + { + wp.set_in_mutex_group(mutex_yaml.as()); + } + const YAML::Node& lift_option = options["lift"]; if (lift_option) { @@ -133,14 +213,31 @@ rmf_traffic::agv::Graph parse_graph( { wps_of_lift[lift_name].push_back(wp.index()); lift_of_wp[wp.index()] = lift_name; + if (has_lifts) + { + const auto lift = graph.find_known_lift(lift_name); + if (!lift) + { + throw std::runtime_error( + "Lift properties for [" + lift_name + "] were not provided " + "even though it is used by a vertex. This suggests that your " + "nav graph was not generated correctly."); + } + wp.set_in_lift(lift); + } } } + + const YAML::Node& merge_radius_option = options["merge_radius"]; + if (merge_radius_option) + { + wp.set_merge_radius(merge_radius_option.as()); + } } const YAML::Node& lanes = level.second["lanes"]; for (const auto& lane : lanes) { - ConstraintPtr constraint = nullptr; const YAML::Node& options = lane[2]; @@ -273,24 +370,36 @@ rmf_traffic::agv::Graph parse_graph( if (const YAML::Node docking_option = options["dock_name"]) { const std::string dock_name = docking_option.as(); - const rmf_traffic::Duration duration = std::chrono::seconds(5); - if (entry_event) + if (!dock_name.empty()) { - // Add a waypoint and a lane leading to it for the dock maneuver - // to be done after the entry event - const auto entry_wp = graph.get_waypoint(begin); - auto& dock_wp = graph.add_waypoint(map_name, entry_wp.get_location()); - - graph.add_lane( - {begin, entry_event}, - {dock_wp.index(), rmf_utils::clone_ptr()}); - - // First lane from start -> dock, second lane from dock -> end - begin = dock_wp.index(); - - vnum_temp++; + const rmf_traffic::Duration duration = std::chrono::seconds(5); + if (entry_event) + { + // Add a waypoint and a lane leading to it for the dock maneuver + // to be done after the entry event + const auto entry_wp = graph.get_waypoint(begin); + auto& dock_wp = + graph.add_waypoint(map_name, entry_wp.get_location()); + dock_wp.set_in_mutex_group(entry_wp.in_mutex_group()); + dock_wp.set_merge_radius(0.0); + + graph.add_lane( + {begin, entry_event}, + {dock_wp.index(), rmf_utils::clone_ptr()}); + stacked_vertex.insert({begin, dock_wp.index()}); + + if (const auto lift = graph.get_waypoint(begin).in_lift()) + { + dock_wp.set_in_lift(lift); + } + + // First lane from start -> dock, second lane from dock -> end + begin = dock_wp.index(); + + vnum_temp++; + } + entry_event = Event::make(Lane::Dock(dock_name, duration)); } - entry_event = Event::make(Lane::Dock(dock_name, duration)); } auto& graph_lane = graph.add_lane( @@ -303,14 +412,21 @@ rmf_traffic::agv::Graph parse_graph( if (speed_limit > 0.0) graph_lane.properties().speed_limit(speed_limit); } + + if (const YAML::Node mutex_yaml = options["mutex"]) + { + graph_lane.properties() + .set_in_mutex_group(mutex_yaml.as()); + } } vnum += vnum_temp; } for (const auto& lift : wps_of_lift) { + double largest_dist = 0.0; const auto& wps = lift.second; - for (std::size_t i = 0; i < wps.size()-1; i++) + for (std::size_t i = 0; i < wps.size()-1; ++i) { rmf_utils::clone_ptr entry_event; rmf_utils::clone_ptr exit_event; @@ -327,6 +443,46 @@ rmf_traffic::agv::Graph parse_graph( graph.add_lane( {wps[i+1], entry_event}, {wps[i], exit_event}); + + const auto pi = graph.get_waypoint(wps[i]).get_location(); + for (std::size_t j = i+1; j < wps.size(); ++j) + { + const auto pj = graph.get_waypoint(wps[j]).get_location(); + const auto dist = (pj - pi).norm(); + if (dist > largest_dist) + largest_dist = dist; + } + } + + if (largest_dist > 0.1) + { + throw std::runtime_error( + "Bad vertical alignment for the waypoints in lift [" + lift.first + + "]. Largest variation is " + std::to_string(largest_dist)); + } + + Eigen::Vector2d lift_center = Eigen::Vector2d::Zero(); + double weight = 0.0; + for (const auto wp : wps) + { + lift_center += graph.get_waypoint(wp).get_location(); + weight += 1.0; + } + + if (weight > 0.0) + { + lift_center /= weight; + for (const auto wp : wps) + { + graph.get_waypoint(wp).set_location(lift_center); + const auto s_it = stacked_vertex.find(wp); + if (s_it != stacked_vertex.end()) + { + std::cout << "Also shifting stacked vertex " << s_it->first << ":" << + s_it->second << std::endl; + graph.get_waypoint(s_it->second).set_location(lift_center); + } + } } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index d0e19cc16..1be09febc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -345,7 +345,6 @@ void MockAdapter::dispatch_task( // NOTE: although the current adapter supports multiple fleets. The test // here assumses using a single fleet for each adapter - bool accepted = false; auto bid = rmf_task_msgs::build() .request(request.dump()) .task_id(task_id) @@ -353,27 +352,31 @@ void MockAdapter::dispatch_task( fimpl.bid_notice_cb( bid, - [&accepted](const rmf_task_ros2::bidding::Response& response) + [fimpl = &fimpl, + task_id](const rmf_task_ros2::bidding::Response& response) { - accepted = response.proposal.has_value(); + if (response.proposal.has_value()) + { + fimpl->worker.schedule([fimpl, task_id](const auto&) + { + rmf_task_msgs::msg::DispatchCommand req; + req.task_id = task_id; + req.fleet_name = fimpl->name; + req.type = req.TYPE_AWARD; + fimpl->dispatch_command_cb( + std::make_shared(req)); + std::cout << "Fleet [" << fimpl->name << + "] accepted the task request" + << std::endl; + }); + } + else + { + std::cout << "Fleet [" << fimpl->name << + "] rejected the task request" + << std::endl; + } }); - - if (accepted) - { - rmf_task_msgs::msg::DispatchCommand req; - req.task_id = task_id; - req.fleet_name = fimpl.name; - req.type = req.TYPE_AWARD; - fimpl.dispatch_command_cb( - std::make_shared(req)); - std::cout << "Fleet [" << fimpl.name << "] accepted the task request" - << std::endl; - } - else - { - std::cout << "Fleet [" << fimpl.name << "] rejected the task request" - << std::endl; - } } }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index d64f29ef2..222df2d6f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -243,7 +243,8 @@ void EmergencyPullover::Active::_find_plan() _state->update_log().info("Searching for an emergency pullover"); _find_pullover_service = std::make_shared( - _context->planner(), _context->location(), _context->schedule()->snapshot(), + _context->emergency_planner(), _context->location(), + _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile()); _pullover_subscription = @@ -335,9 +336,38 @@ void EmergencyPullover::Active::_execute_plan( if (_is_interrupted) return; + if (plan.get_itinerary().empty() || plan.get_waypoints().empty()) + { + _state->update_status(Status::Completed); + _state->update_log().info( + "The planner indicates that the robot is already in a pullover spot."); + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is already in a pullover spot", + _context->requester_id().c_str()); + + _finished(); + return; + } + + if (!plan.get_waypoints().back().graph_index().has_value()) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] has no graph index for its final waypoint. This is a serious " + "bug and should be reported to the RMF maintainers.", + _context->requester_id().c_str()); + _schedule_retry(); + return; + } + + auto goal = rmf_traffic::agv::Plan::Goal( + plan.get_waypoints().back().graph_index().value()); + _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), std::move(full_itinerary), _assign_id, - _state, _update, _finished, std::nullopt); + _context, plan_id, std::move(plan), std::move(goal), + std::move(full_itinerary), _assign_id, _state, _update, + _finished, std::nullopt); if (!_execution.has_value()) { @@ -371,7 +401,7 @@ Negotiator::NegotiatePtr EmergencyPullover::Active::_respond( const auto evaluator = Negotiator::make_evaluator(table_view); return services::Negotiate::emergency_pullover( - _context->itinerary().assign_plan_id(), _context->planner(), + _context->itinerary().assign_plan_id(), _context->emergency_planner(), _context->location(), table_view, responder, std::move(approval_cb), std::move(evaluator)); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 75a5160e0..e8127f8ef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -19,6 +19,7 @@ #include "LegacyPhaseShim.hpp" #include "WaitForTraffic.hpp" #include "WaitUntil.hpp" +#include "LockMutexGroup.hpp" #include "../phases/MoveRobot.hpp" #include "../phases/DoorOpen.hpp" @@ -26,6 +27,8 @@ #include "../phases/RequestLift.hpp" #include "../phases/DockRobot.hpp" +#include "../agv/internal_EasyFullControl.hpp" + #include namespace rmf_fleet_adapter { @@ -37,16 +40,29 @@ using StandbyPtr = rmf_task_sequence::Event::StandbyPtr; using UpdateFn = std::function; using MakeStandby = std::function; +std::string print_plan_waypoints( + const std::vector& waypoints, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + for (const auto& wp : waypoints) + ss << "\n -- " << agv::print_plan_waypoint(wp, graph, + waypoints.front().time()); + return ss.str(); +} + //============================================================================== struct LegacyPhaseWrapper { LegacyPhaseWrapper( std::shared_ptr phase_, rmf_traffic::Time time_, - rmf_traffic::Dependencies dependencies_) + rmf_traffic::Dependencies dependencies_, + std::optional mutex_group_dependency_) : phase(std::move(phase_)), time(time_), - dependencies(std::move(dependencies_)) + dependencies(std::move(dependencies_)), + mutex_group_dependency(std::move(mutex_group_dependency_)) { // Do nothing } @@ -54,6 +70,7 @@ struct LegacyPhaseWrapper std::shared_ptr phase; rmf_traffic::Time time; rmf_traffic::Dependencies dependencies; + std::optional mutex_group_dependency; }; using LegacyPhases = std::vector; @@ -65,10 +82,22 @@ using RequestLift = phases::RequestLift::PendingPhase; using EndLift = phases::EndLiftSession::Pending; using Move = phases::MoveRobot::PendingPhase; +//============================================================================== +MakeStandby make_wait_for_mutex( + const agv::RobotContextPtr& context, + const rmf_task_sequence::Event::AssignIDPtr& id, + const LockMutexGroup::Data& data) +{ + return [context, id, data](UpdateFn update) + { + return LockMutexGroup::Standby::make(context, id, data); + }; +} + //============================================================================== MakeStandby make_wait_for_traffic( const agv::RobotContextPtr& context, - const rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, const rmf_traffic::Dependencies& deps, const rmf_traffic::Time time, const rmf_task_sequence::Event::AssignIDPtr& id) @@ -80,6 +109,42 @@ MakeStandby make_wait_for_traffic( }; } +//============================================================================== +void truncate_arrival( + rmf_traffic::schedule::Itinerary& previous_itinerary, + const rmf_traffic::agv::Plan::Waypoint& wp) +{ + std::size_t first_excluded_route = 0; + for (const auto& c : wp.arrival_checkpoints()) + { + first_excluded_route = std::max(first_excluded_route, c.route_id+1); + auto& r = previous_itinerary.at(c.route_id); + auto& t = r.trajectory(); + + t.erase(t.begin() + (int)c.checkpoint_id, t.end()); + } + + for (std::size_t i = 0; i < previous_itinerary.size(); ++i) + { + const auto& t = previous_itinerary.at(i).trajectory(); + if (t.size() < 2) + { + // If we've reduced this trajectory down to nothing, then erase + // it and all later routes. In the current version of RMF + // we assume that routes with higher indices will never have an + // earlier time value than the earliest of a lower index route. + // This is an assumption we should relax in the future, but it + // helps here for now. + first_excluded_route = + std::min(first_excluded_route, i); + } + } + + previous_itinerary.erase( + previous_itinerary.begin()+first_excluded_route, + previous_itinerary.end()); +} + //============================================================================== class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor { @@ -90,25 +155,57 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor EventPhaseFactory( agv::RobotContextPtr context, LegacyPhases& phases, - rmf_traffic::Time event_start_time, - const rmf_traffic::Dependencies& dependencies, + const rmf_traffic::agv::Plan::Waypoint& initial_waypoint_, + std::optional next_waypoint_, + const PlanIdPtr plan_id, + std::function&, + const rmf_traffic::agv::Plan::Waypoint&)> make_current_mutex_groups, + std::function>( + const rmf_traffic::agv::Plan::Waypoint&)> get_new_mutex_groups, + std::shared_ptr& previous_itinerary, + const rmf_traffic::schedule::Itinerary& full_itinerary, bool& continuous) - : _context(std::move(context)), + : initial_waypoint(initial_waypoint_), + next_waypoint(std::move(next_waypoint_)), + _context(std::move(context)), _phases(phases), - _event_start_time(event_start_time), - _dependencies(dependencies), + _event_start_time(initial_waypoint_.time()), + _plan_id(plan_id), + _make_current_mutex_groups(std::move(make_current_mutex_groups)), + _get_new_mutex_group(std::move(get_new_mutex_groups)), + _previous_itinerary(previous_itinerary), + _full_itinerary(full_itinerary), _continuous(continuous) { // Do nothing } + rmf_traffic::agv::Plan::Waypoint initial_waypoint; + std::optional next_waypoint; + void execute(const Dock& dock) final { + std::optional event_mutex_group; + if (next_waypoint.has_value() && next_waypoint->graph_index().has_value()) + { + const auto [mutex_group_change, new_mutex_groups] = + _get_new_mutex_group(*next_waypoint); + + if (mutex_group_change) + { + event_mutex_group = _make_current_mutex_groups( + new_mutex_groups, initial_waypoint); + } + } + assert(!_moving_lift); _phases.emplace_back( std::make_shared( - _context, dock.dock_name()), - _event_start_time, _dependencies); + _context, dock.dock_name(), + next_waypoint.value_or(initial_waypoint), + _plan_id), + _event_start_time, initial_waypoint.dependencies(), event_mutex_group); _continuous = false; } @@ -122,7 +219,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.name(), _context->requester_id(), _event_start_time + open.duration()), - _event_start_time, _dependencies); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -137,12 +234,17 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, _dependencies); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } void execute(const LiftSessionBegin& open) final { + truncate_arrival(*_previous_itinerary, initial_waypoint); + + _previous_itinerary = + std::make_shared(_full_itinerary); + assert(!_moving_lift); const auto node = _context->node(); _phases.emplace_back( @@ -150,9 +252,15 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, open.lift_name(), open.floor_name(), - _event_start_time, - phases::RequestLift::Located::Outside), - _event_start_time, _dependencies); + phases::RequestLift::Data{ + initial_waypoint.time(), + phases::RequestLift::Located::Outside, + _plan_id, + std::nullopt, + _previous_itinerary, + initial_waypoint + }), + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -169,7 +277,29 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftDoorOpen& open) final { + std::string name; + const auto& graph = _context->navigation_graph(); + const auto i1 = initial_waypoint.graph_index(); + if (const auto nav_params = _context->nav_params()) + { + name = nav_params->get_vertex_name(graph, i1); + } + else if (i1.has_value()) + { + if (const std::string* n = graph.get_waypoint(i1.value()).name()) + { + name = *n; + } + } + const auto node = _context->node(); + auto localize = agv::Destination::Implementation::make( + open.floor_name(), + initial_waypoint.position(), + initial_waypoint.graph_index(), + name, + std::nullopt, + nullptr); // TODO(MXG): The time calculation here should be considered more carefully. _phases.emplace_back( @@ -177,9 +307,13 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, open.lift_name(), open.floor_name(), - _event_start_time + open.duration() + _lifting_duration, - phases::RequestLift::Located::Inside), - _event_start_time, _dependencies); + phases::RequestLift::Data{ + _event_start_time + open.duration() + _lifting_duration, + phases::RequestLift::Located::Inside, + _plan_id, + localize + }), + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _moving_lift = false; _continuous = true; @@ -194,7 +328,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, _dependencies); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -213,13 +347,19 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor agv::RobotContextPtr _context; LegacyPhases& _phases; rmf_traffic::Time _event_start_time; - const rmf_traffic::Dependencies& _dependencies; + PlanIdPtr _plan_id; + std::function&, + const rmf_traffic::agv::Plan::Waypoint&)> _make_current_mutex_groups; + std::function>( + const rmf_traffic::agv::Plan::Waypoint&)> _get_new_mutex_group; + std::shared_ptr& _previous_itinerary; + const rmf_traffic::schedule::Itinerary& _full_itinerary; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); }; - //============================================================================== struct EventGroupInfo { @@ -232,7 +372,7 @@ std::optional search_for_door_group( LegacyPhases::const_iterator head, LegacyPhases::const_iterator end, const agv::RobotContextPtr& context, - const rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, const rmf_task::Event::AssignIDPtr& id) { const auto* door_open = dynamic_cast( @@ -268,12 +408,21 @@ std::optional search_for_door_group( ++tail; for (auto it = head; it != tail; ++it) { - door_group.push_back( - [legacy = it->phase, context, id](UpdateFn update) - { - return LegacyPhaseShim::Standby::make( - legacy, context->worker(), context->clock(), id, update); - }); + if (it->mutex_group_dependency.has_value()) + { + door_group.push_back(make_wait_for_mutex( + context, id, it->mutex_group_dependency.value())); + } + + if (it->phase) + { + door_group.push_back( + [legacy = it->phase, context, id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), id, update); + }); + } if (!it->dependencies.empty()) { @@ -323,7 +472,7 @@ std::optional search_for_lift_group( LegacyPhases::const_iterator head, LegacyPhases::const_iterator end, const agv::RobotContextPtr& context, - const rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, const rmf_task::Event::AssignIDPtr& event_id, const rmf_task::events::SimpleEventStatePtr& state) { @@ -377,12 +526,21 @@ std::optional search_for_lift_group( ++tail; for (auto it = head; it != tail; ++it) { - lift_group.push_back( - [legacy = it->phase, context, event_id](UpdateFn update) - { - return LegacyPhaseShim::Standby::make( - legacy, context->worker(), context->clock(), event_id, update); - }); + if (it->mutex_group_dependency.has_value()) + { + lift_group.push_back(make_wait_for_mutex( + context, event_id, it->mutex_group_dependency.value())); + } + + if (it->phase) + { + lift_group.push_back( + [legacy = it->phase, context, event_id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), event_id, update); + }); + } if (!it->dependencies.empty()) { @@ -421,11 +579,84 @@ std::optional search_for_lift_group( } // anonymous namespace +//============================================================================== +class LiftFinder : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + LiftFinder(std::string current_name_) + : current_name(std::move(current_name_)), + still_using(false) + { + // Do nothing + } + std::string current_name; + bool still_using = false; + + void execute(const Dock& dock) final {} + void execute(const Wait&) final {} + void execute(const DoorOpen&) final {} + void execute(const DoorClose&) final {} + void execute(const LiftSessionBegin& e) final + { + // If we're going to re-begin using a lift, then we don't need to keep this + // one locked. The new LiftSessionBegin event will re-lock the session. + if (e.lift_name() == current_name) + still_using = true; + } + void execute(const LiftMove& e) final + { + if (e.lift_name() == current_name) + still_using = true; + } + void execute(const LiftDoorOpen& e) final + { + if (e.lift_name() == current_name) + still_using = true; + } + void execute(const LiftSessionEnd& e) final + { + if (e.lift_name() == current_name) + still_using = true; + } +}; + +//============================================================================== +class DoorFinder : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + DoorFinder(std::string current_name_) + : current_name(std::move(current_name_)), + still_using(false) + { + // Do nothing + } + std::string current_name; + bool still_using; + + void execute(const Dock& dock) final {} + void execute(const Wait&) final {} + void execute(const DoorOpen& open) final + { + if (open.name() == current_name) + still_using = true; + } + void execute(const DoorClose& close) final + { + if (close.name() == current_name) + still_using = true; + } + void execute(const LiftSessionBegin& e) final {} + void execute(const LiftMove& e) final {} + void execute(const LiftDoorOpen& e) final {} + void execute(const LiftSessionEnd& e) final {} +}; + //============================================================================== std::optional ExecutePlan::make( agv::RobotContextPtr context, - rmf_traffic::PlanId plan_id, + rmf_traffic::PlanId recommended_plan_id, rmf_traffic::agv::Plan plan, + rmf_traffic::agv::Plan::Goal goal, rmf_traffic::schedule::Itinerary full_itinerary, const rmf_task::Event::AssignIDPtr& event_id, rmf_task::events::SimpleEventStatePtr state, @@ -433,6 +664,165 @@ std::optional ExecutePlan::make( std::function finished, std::optional tail_period) { + if (plan.get_waypoints().empty()) + return std::nullopt; + + auto plan_id = std::make_shared(recommended_plan_id); + const auto& graph = context->navigation_graph(); + LegacyPhases legacy_phases; + + rmf_traffic::agv::Graph::LiftPropertiesPtr release_lift; + const auto envelope = context->profile()->footprint() + ->get_characteristic_length()/2.0; + if (const auto* current_lift = context->current_lift_destination()) + { + LiftFinder finder(current_lift->lift_name); + for (const auto& wp : plan.get_waypoints()) + { + if (wp.event()) + { + wp.event()->execute(finder); + if (finder.still_using) + break; + } + } + + if (!finder.still_using) + { + const auto found_lift = graph.find_known_lift(current_lift->lift_name); + if (found_lift) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Robot [%s] will release lift [%s] after a replan because it is no " + "longer needed.", + context->requester_id().c_str(), + current_lift->lift_name.c_str()); + + release_lift = found_lift; + } + else + { + std::stringstream ss; + for (const auto& l : graph.all_known_lifts()) + { + ss << "[" << l->name() << "]"; + } + + RCLCPP_ERROR( + context->node()->get_logger(), + "Robot [%s] might be stuck with locking an unknown lift named [%s]. " + "Known lifts include %s", + context->requester_id().c_str(), + current_lift->lift_name.c_str(), + ss.str().c_str()); + } + } + } + + if (!plan.get_waypoints().front().graph_index().has_value()) + { + const Eigen::Vector2d p0 = + plan.get_waypoints().front().position().block<2, 1>(0, 0); + const auto t0 = plan.get_waypoints().front().time(); + + const auto first_graph_wp = [&]() -> std::optional + { + for (const auto& wp : plan.get_waypoints()) + { + if (wp.graph_index().has_value()) + return *wp.graph_index(); + } + + return std::nullopt; + }(); + + if (first_graph_wp.has_value()) + { + const Eigen::Vector2d p1 = + graph.get_waypoint(*first_graph_wp).get_location(); + + // Check if the line from the start of the plan to this waypoint crosses + // through a door, and add a DoorOpen phase if it does + for (const auto& door : graph.all_known_doors()) + { + + if (door->intersects(p0, p1, envelope)) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Ensuring door [%s] is open for [%s] after a replan", + door->name().c_str(), + context->requester_id().c_str()); + + legacy_phases.emplace_back( + std::make_shared( + context, door->name(), context->requester_id(), t0), + t0, rmf_traffic::Dependencies(), std::nullopt); + } + } + + const auto& map = graph.get_waypoint(*first_graph_wp).get_map_name(); + // Check if the robot is going into a lift and summon the lift + for (const auto& lift : graph.all_known_lifts()) + { + if (lift->is_in_lift(p1, envelope)) + { + auto side = phases::RequestLift::Located::Outside; + if (lift->is_in_lift(p0, envelope)) + { + side = phases::RequestLift::Located::Inside; + } + + RCLCPP_INFO( + context->node()->get_logger(), + "Robot [%s] will summon lift [%s] to floor [%s] after a replan", + context->requester_id().c_str(), + lift->name().c_str(), + map.c_str()); + + legacy_phases.emplace_back( + std::make_shared( + context, lift->name(), map, + phases::RequestLift::Data{t0, side, plan_id}), + t0, rmf_traffic::Dependencies(), std::nullopt); + } + } + } + } + + std::optional release_door; + if (context->holding_door().has_value()) + { + const auto& current_door = *context->holding_door(); + DoorFinder finder(current_door); + for (const auto& wp : plan.get_waypoints()) + { + if (wp.event()) + { + wp.event()->execute(finder); + if (finder.still_using) + break; + } + } + + if (!finder.still_using) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Robot [%s] will release door [%s] after a replan because it is no " + "longer needed.", + context->requester_id().c_str(), + current_door.c_str()); + release_door = current_door; + } + } + + auto initial_itinerary = + std::make_shared(full_itinerary); + auto previous_itinerary = initial_itinerary; + + std::optional finish_time_estimate; for (const auto& r : plan.get_itinerary()) { @@ -452,54 +842,275 @@ std::optional ExecutePlan::make( plan.get_waypoints(); std::vector move_through; + std::optional current_mutex_groups; + std::unordered_set remaining_mutex_groups; - LegacyPhases legacy_phases; + const auto make_current_mutex_groups = [&]( + const std::unordered_set& new_mutex_groups, + const rmf_traffic::agv::Plan::Waypoint& wp) + { + remaining_mutex_groups = new_mutex_groups; + const rmf_traffic::Time hold_time = wp.time(); + const Eigen::Vector3d hold_position = wp.position(); + std::string hold_map; + if (wp.graph_index().has_value()) + { + hold_map = + graph.get_waypoint(*wp.graph_index()).get_map_name(); + } + else + { + // Find the map name of the first waypoint that is on the graph + for (const auto& wp : waypoints) + { + if (wp.graph_index().has_value()) + { + hold_map = + graph.get_waypoint(*wp.graph_index()).get_map_name(); + break; + } + } + + if (hold_map.empty()) + { + RCLCPP_ERROR( + context->node()->get_logger(), + "Cannot find a map for a mutex group transition needed " + "by robot [%s]. There are [%lu] remaining waypoints. Please " + "report this situation to the maintainers of RMF.", + context->requester_id().c_str(), + waypoints.size()); + } + } + + truncate_arrival(*previous_itinerary, wp); + + auto expected_waypoints = waypoints; + expected_waypoints.insert(expected_waypoints.begin(), wp); + + auto next_itinerary = std::make_shared< + rmf_traffic::schedule::Itinerary>(full_itinerary); + auto data = LockMutexGroup::Data{ + new_mutex_groups, + hold_map, + hold_position, + hold_time, + plan_id, + next_itinerary, + expected_waypoints, + goal + }; + + previous_itinerary = data.resume_itinerary; + + return data; + }; + + const auto get_new_mutex_groups = [&]( + const rmf_traffic::agv::Plan::Waypoint& wp) + { + std::unordered_set new_mutex_groups; + if (wp.graph_index().has_value()) + { + const auto& group = + graph.get_waypoint(*wp.graph_index()).in_mutex_group(); + if (!group.empty()) + { + new_mutex_groups.insert(group); + } + } + + for (const auto l : wp.approach_lanes()) + { + const auto& lane = graph.get_lane(l); + const auto& group = lane.properties().in_mutex_group(); + if (!group.empty()) + { + new_mutex_groups.insert(group); + break; + } + } + + bool mutex_group_change = + (!new_mutex_groups.empty() && remaining_mutex_groups.empty()); + + if (!mutex_group_change && !remaining_mutex_groups.empty()) + { + for (const auto& new_group : new_mutex_groups) + { + if (remaining_mutex_groups.count(new_group) == 0) + { + mutex_group_change = true; + break; + } + } + } + + if (!mutex_group_change) + { + // We don't need to lock any new mutex groups, but we may be releasing + // some. We should reduce the remaining group set to whatever is in the + // new group set so that if we need to add some groups back later then + // we recognize it. + remaining_mutex_groups = new_mutex_groups; + } + + return std::make_pair(mutex_group_change, new_mutex_groups); + }; + + const auto t0 = waypoints.front().time(); while (!waypoints.empty()) { auto it = waypoints.begin(); bool event_occurred = false; for (; it != waypoints.end(); ++it) { + const auto [mutex_group_change, new_mutex_groups] = get_new_mutex_groups( + *it); + if (mutex_group_change) + { + if (move_through.size() > 1) + { + auto next_mutex_group = make_current_mutex_groups( + new_mutex_groups, move_through.back()); + + legacy_phases.emplace_back( + std::make_shared( + context, move_through, plan_id, tail_period), + move_through.back().time(), move_through.back().dependencies(), + current_mutex_groups); + + auto last = move_through.back(); + move_through.clear(); + // Repeat the last waypoint so that follow_new_path has continuity. + move_through.push_back(last); + waypoints.erase(waypoints.begin(), it); + + current_mutex_groups = next_mutex_group; + + // We treat this the same as an event occurring to indicate that + // we should keep looping. + event_occurred = true; + break; + } + else + { + // We don't need to put in a break because nothing has been moved + // through yet. Just set the current_mutex_groups from this point. + if (move_through.empty()) + { + current_mutex_groups = make_current_mutex_groups( + new_mutex_groups, *it); + } + else + { + assert(move_through.size() == 1); + current_mutex_groups = make_current_mutex_groups( + new_mutex_groups, move_through.back()); + } + } + } + move_through.push_back(*it); + const bool release_lift_here = release_lift && + it->graph_index().has_value() && + !release_lift->is_in_lift(it->position().block<2, 1>(0, 0), envelope); - if (it->event()) + const bool release_door_here = release_door.has_value() + && it->graph_index().has_value(); + + if (it->event() || release_lift_here || release_door_here) { if (move_through.size() > 1) { legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies()); + it->time(), it->dependencies(), current_mutex_groups); + } + else if (current_mutex_groups.has_value()) + { + legacy_phases.emplace_back( + nullptr, it->time(), + rmf_traffic::Dependencies(), current_mutex_groups); + } + current_mutex_groups = std::nullopt; + + if (release_lift_here) + { + legacy_phases.emplace_back( + std::make_shared( + context, + release_lift->name(), + ""), + it->time(), rmf_traffic::Dependencies(), std::nullopt); + + release_lift = nullptr; + } + + if (release_door_here) + { + legacy_phases.emplace_back( + std::make_shared( + context, + *release_door, + context->requester_id()), + it->time(), rmf_traffic::Dependencies(), std::nullopt); + + release_door = std::nullopt; + } + + std::optional next_waypoint; + auto next_it = it + 1; + if (next_it != waypoints.end()) + { + next_waypoint = *next_it; } move_through.clear(); bool continuous = true; - EventPhaseFactory factory( - context, legacy_phases, it->time(), it->dependencies(), continuous); - it->event()->execute(factory); - while (factory.moving_lift()) + if (it->event()) { - const auto last_it = it; - ++it; - if (!it->event()) + EventPhaseFactory factory( + context, legacy_phases, *it, next_waypoint, plan_id, + make_current_mutex_groups, get_new_mutex_groups, + previous_itinerary, full_itinerary, + continuous); + + it->event()->execute(factory); + while (factory.moving_lift()) { - const double dist = - (it->position().block<2, 1>(0, 0) - - last_it->position().block<2, 1>(0, 0)).norm(); + const auto last_it = it; + ++it; + if (it == waypoints.end()) + { + // This should not happen... this would imply that the goal was + // inside of a lift + return std::nullopt; + } - if (dist < 0.5) + if (!it->event()) { - // We'll assume that this is just a misalignment in the maps + const double dist = + (it->position().block<2, 1>(0, 0) + - last_it->position().block<2, 1>(0, 0)).norm(); + + if (dist > 0.5) + { + // We'll assume that this is just a misalignment in the maps + state->update_log().warn( + "Plan involves a translation of [" + std::to_string( + dist) + + "m] while inside a lift. This may indicate an error in the " + "navigation graph. Please report this to the system integrator."); + } + continue; } - state->update_log().warn( - "Plan involves a translation of [" + std::to_string(dist) - + "m] while inside a lift. This may indicate an error in the " - "navigation graph. Please report this to the system integrator."); + factory.initial_waypoint = *it; + it->event()->execute(factory); } - - it->event()->execute(factory); } if (continuous) @@ -520,11 +1131,12 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies()); + it->time(), it->dependencies(), current_mutex_groups); } else { - legacy_phases.emplace_back(nullptr, it->time(), it->dependencies()); + legacy_phases.emplace_back( + nullptr, it->time(), it->dependencies(), current_mutex_groups); } // Have the next sequence of waypoints begin with this one. @@ -548,7 +1160,8 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - finish_time_estimate.value(), rmf_traffic::Dependencies{}); + finish_time_estimate.value(), rmf_traffic::Dependencies{}, + current_mutex_groups); } if (!event_occurred) @@ -583,6 +1196,12 @@ std::optional ExecutePlan::make( } else { + if (head->mutex_group_dependency.has_value()) + { + standbys.push_back(make_wait_for_mutex( + context, event_id, head->mutex_group_dependency.value())); + } + if (head->phase) { standbys.push_back( @@ -618,49 +1237,63 @@ std::optional ExecutePlan::make( }); } - auto sequence = rmf_task_sequence::events::Bundle::standby( - rmf_task_sequence::events::Bundle::Type::Sequence, - standbys, state, std::move(update))->begin([]() {}, std::move(finished)); - std::size_t attempts = 0; - while (!context->itinerary().set(plan_id, std::move(full_itinerary))) - { - // Some mysterious behavior has been happening where plan_ids are invalid. - // We will attempt to catch that here and try to learn more about what - // could be causing that, while allowing progress to continue. - std::string task_id = ""; - if (context->current_task_id()) - task_id = *context->current_task_id(); - - RCLCPP_ERROR( - context->node()->get_logger(), - "Invalid plan_id [%lu] when current plan_id is [%lu] for [%s] in group " - "[%s] while performing task [%s]. Please notify an RMF developer.", - plan_id, - context->itinerary().current_plan_id(), - context->name().c_str(), - context->group().c_str(), - task_id.c_str()); - state->update_log().error( - "Invalid plan_id [" + std::to_string(plan_id) + "] when current plan_id " - "is [" + std::to_string(context->itinerary().current_plan_id()) + "] " - "Please notify an RMF developer."); - - plan_id = context->itinerary().assign_plan_id(); - - if (++attempts > 5) + if (!initial_itinerary->empty()) + { + while (!context->itinerary().set(*plan_id, *initial_itinerary)) { + // Some mysterious behavior has been happening where plan_ids are invalid. + // We will attempt to catch that here and try to learn more about what + // could be causing that, while allowing progress to continue. + std::string task_id = ""; + if (context->current_task_id()) + task_id = *context->current_task_id(); + RCLCPP_ERROR( context->node()->get_logger(), - "Requesting replan for [%s] in group [%s] because plan is repeatedly " - "being rejected while performing task [%s]", + "Failed to set plan_id [%lu] when current plan_id is [%lu] for [%s] in " + "group [%s] while executing plan for task [%s]. Please report this bug " + "to the RMF maintainers.", + *plan_id, + context->itinerary().current_plan_id(), context->name().c_str(), context->group().c_str(), task_id.c_str()); - return std::nullopt; + state->update_log().error( + "Invalid plan_id [" + std::to_string(*plan_id) + + "] when current plan_id is [" + + std::to_string(context->itinerary().current_plan_id()) + + "] Please notify an RMF developer."); + + *plan_id = context->itinerary().assign_plan_id(); + + if (++attempts > 5) + { + std::stringstream ss_sizes; + for (const auto& r : *initial_itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + + RCLCPP_ERROR( + context->node()->get_logger(), + "Requesting replan for [%s] in group [%s] because plan is repeatedly " + "being rejected while performing task [%s]. Itinerary has [%lu] " + "with sizes of %s.", + context->name().c_str(), + context->group().c_str(), + task_id.c_str(), + initial_itinerary->size(), + ss_sizes.str().c_str()); + return std::nullopt; + } } } + auto sequence = rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + standbys, state, std::move(update))->begin([]() {}, std::move(finished)); + return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp index 615ccffe6..703349d35 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp @@ -19,6 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP #include "../agv/RobotContext.hpp" +#include "../LegacyTask.hpp" #include #include @@ -33,6 +34,7 @@ struct ExecutePlan agv::RobotContextPtr context, rmf_traffic::PlanId plan_id, rmf_traffic::agv::Plan plan, + rmf_traffic::agv::Plan::Goal goal, rmf_traffic::schedule::Itinerary full_itinerary, const rmf_task_sequence::Event::AssignIDPtr& event_id, rmf_task::events::SimpleEventStatePtr state, @@ -41,7 +43,7 @@ struct ExecutePlan std::optional tail_period); rmf_traffic::agv::Plan plan; - rmf_traffic::PlanId plan_id; + PlanIdPtr plan_id; rmf_traffic::Time finish_time_estimate; rmf_task_sequence::Event::ActivePtr sequence; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 08025eb9b..66712eb04 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -279,8 +279,19 @@ rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const const auto now = _context->now(); const auto& itin = _context->itinerary(); - if (const auto delay = itin.cumulative_delay(_execution->plan_id)) - return finish - now + *delay; + if (_execution->plan_id) + { + if (const auto delay = itin.cumulative_delay(*_execution->plan_id)) + return finish - now + *delay; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Missing plan_id for go_to_place of robot [%s]. Please report this " + "critical bug to the maintainers of RMF.", + _context->requester_id().c_str()); + } } if (!_chosen_goal.has_value()) @@ -507,6 +518,17 @@ void GoToPlace::Active::_find_plan() _state->update_log().info( "Generating plan to move from [" + start_name + "] to [" + goal_name + "]"); + const auto& graph = _context->navigation_graph(); + std::stringstream ss; + ss << "Planning for [" << _context->requester_id() + << "] to [" << goal_name << "] from one of these locations:" + << agv::print_starts(_context->location(), graph); + + RCLCPP_INFO( + _context->node()->get_logger(), + "%s", + ss.str().c_str()); + // TODO(MXG): Make the planning time limit configurable _find_path_service = std::make_shared( _context->planner(), _context->location(), *_chosen_goal, @@ -621,6 +643,16 @@ void GoToPlace::Active::_execute_plan( _state->update_status(Status::Completed); _state->update_log().info( "The planner indicates that the robot is already at its goal."); + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is already at its goal [%lu]", + _context->requester_id().c_str(), + _goal.waypoint()); + + const auto& graph = _context->navigation_graph(); + _context->retain_mutex_groups( + {graph.get_waypoint(_goal.waypoint()).in_mutex_group()}); + _finished(); return; } @@ -635,7 +667,7 @@ void GoToPlace::Active::_execute_plan( _context->requester_id().c_str()); _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), std::move(full_itinerary), + _context, plan_id, std::move(plan), _goal, std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); if (!_execution.has_value()) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp new file mode 100644 index 000000000..8c1dba8f1 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -0,0 +1,401 @@ +/* + * Copyright (C) 2023 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 "LockMutexGroup.hpp" +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +std::string all_str(const std::unordered_set& all) +{ + std::stringstream ss; + for (const auto& item : all) + { + ss << "[" << item << "]"; + } + return ss.str(); +} + +//============================================================================== +std::string LockMutexGroup::Data::all_groups_str() const +{ + return all_str(mutex_groups); +} + +//============================================================================== +auto LockMutexGroup::Standby::make( + agv::RobotContextPtr context, + const AssignIDPtr& id, + Data data) +-> std::shared_ptr +{ + auto standby = std::shared_ptr(new Standby(std::move(data))); + standby->_context = std::move(context); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + "Lock mutex groups " + data.all_groups_str(), + "Waiting for the mutex groups to be locked", + rmf_task::Event::Status::Standby, {}, standby->_context->clock()); + return standby; +} + +//============================================================================== +auto LockMutexGroup::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration LockMutexGroup::Standby::duration_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto LockMutexGroup::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + return Active::make(_context, _state, std::move(finished), _data); +} + +//============================================================================== +LockMutexGroup::Standby::Standby(Data data) +: _data(data) +{ + // Do nothing +} + +//============================================================================== +auto LockMutexGroup::Active::make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + Data data) -> std::shared_ptr +{ + auto active = std::shared_ptr(new Active(std::move(data))); + active->_context = std::move(context); + active->_state = std::move(state); + active->_finished = std::move(finished); + active->_initialize(); + + return active; +} + +//============================================================================== +auto LockMutexGroup::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration LockMutexGroup::Active::remaining_time_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto LockMutexGroup::Active::backup() const -> Backup +{ + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto LockMutexGroup::Active::interrupt( + std::function task_is_interrupted) -> Resume +{ + _context->worker().schedule([task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + return Resume::make([]() { /* do nothing */ }); +} + +//============================================================================== +void LockMutexGroup::Active::cancel() +{ + _state->update_status(Status::Canceled); + const auto finished = _finished; + _finished = nullptr; + _context->worker().schedule([finished](const auto&) + { + finished(); + }); +} + +//============================================================================== +void LockMutexGroup::Active::kill() +{ + cancel(); +} + +//============================================================================== +LockMutexGroup::Active::Active(Data data) +: _data(std::move(data)) +{ + // Do nothing +} + +//============================================================================== +void LockMutexGroup::Active::_initialize() +{ + _state->update_status(State::Status::Underway); + using MutexGroupStatesPtr = + std::shared_ptr; + + _remaining = _data.mutex_groups; + for (const auto& [locked, _] : _context->locked_mutex_groups()) + { + _remaining.erase(locked); + } + + if (_remaining.empty()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "All mutex groups were already locked for [%s]", + _context->requester_id().c_str()); + + _schedule(*_data.resume_itinerary); + // We don't need to do anything further, we already got the mutex group + // previously. + _context->worker().schedule( + [state = _state, finished = _finished](const auto&) + { + state->update_status(State::Status::Completed); + finished(); + }); + return; + } + + *_data.plan_id += 1; + _context->schedule_hold( + _data.plan_id, + _data.hold_time, + std::chrono::seconds(5), + _data.hold_position, + _data.hold_map); + _stubborn = _context->be_stubborn(); + + _state->update_log().info( + "Waiting to lock mutex group " + _data.all_groups_str()); + RCLCPP_INFO( + _context->node()->get_logger(), + "Waiting to lock mutex groups %s for robot [%s]", + _data.all_groups_str().c_str(), + _context->requester_id().c_str()); + + const auto cumulative_delay = _context->now() - _data.hold_time; + _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); + + _delay_timer = _context->node()->try_create_wall_timer( + std::chrono::seconds(1), + [weak = weak_from_this(), plan_id = *_data.plan_id]() + { + const auto self = weak.lock(); + if (!self) + return; + + self->_apply_cumulative_delay(); + }); + + _listener = _context->request_mutex_groups( + _data.mutex_groups, _data.hold_time) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe([w = weak_from_this()](const std::string& locked) + { + const auto self = w.lock(); + if (!self) + return; + + self->_remaining.erase(locked); + if (self->_remaining.empty()) + { + const auto finished = self->_finished; + self->_finished = nullptr; + if (!finished) + return; + + const auto now = self->_context->now(); + const auto delay = now - self->_data.hold_time; + if (delay > std::chrono::seconds(2)) + { + const auto start = [&]() + -> std::optional + { + for (const auto& wp : self->_data.waypoints) + { + if (wp.graph_index().has_value()) + { + return rmf_traffic::agv::Plan::Start( + self->_context->now(), + *wp.graph_index(), + wp.position()[2]); + } + } + return std::nullopt; + }(); + + if (start.has_value()) + { + self->_find_path_service = std::make_shared( + self->_context->planner(), + std::vector({*start}), + self->_data.goal, + self->_context->schedule()->snapshot(), + self->_context->itinerary().id(), + self->_context->profile(), + std::chrono::seconds(5)); + + self->_plan_subscription = + rmf_rxcpp::make_job( + self->_find_path_service) + .observe_on( + rxcpp::identity_same_worker(self->_context->worker())) + .subscribe( + [w = self->weak_from_this(), finished]( + const services::FindPath::Result& result) + { + const auto self = w.lock(); + if (!self) + return; + + if (self->_consider_plan_result(result)) + { + // We have a matching plan so proceed + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Finished locking mutexes %s for [%s] and plan is " + "unchanged after waiting", + self->_data.all_groups_str().c_str(), + self->_context->requester_id().c_str()); + + self->_schedule(*self->_data.resume_itinerary); + self->_apply_cumulative_delay(); + self->_state->update_status(Status::Completed); + finished(); + return; + } + + // The new plan was not a match, so we should trigger a + // proper replan. + self->_state->update_status(Status::Completed); + self->_context->request_replan(); + }); + + self->_find_path_timeout = + self->_context->node()->try_create_wall_timer( + std::chrono::seconds(10), + [ + weak_service = self->_find_path_service->weak_from_this() + ]() + { + if (const auto service = weak_service.lock()) + { + service->interrupt(); + } + }); + return; + } + } + + // We locked the mutex quickly enough that we should proceed. + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Finished locking mutexes %s for [%s]", + self->_data.all_groups_str().c_str(), + self->_context->requester_id().c_str()); + + self->_schedule(*self->_data.resume_itinerary); + self->_apply_cumulative_delay(); + self->_state->update_status(Status::Completed); + finished(); + return; + } + }); +} + +//============================================================================== +void LockMutexGroup::Active::_schedule( + rmf_traffic::schedule::Itinerary itinerary) const +{ + _context->schedule_itinerary(_data.plan_id, std::move(itinerary)); +} + +//============================================================================== +void LockMutexGroup::Active::_apply_cumulative_delay() +{ + const auto cumulative_delay = _context->now() - _data.hold_time; + _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); +} + +//============================================================================== +namespace { +std::vector filter_graph_indices( + const std::vector& waypoints) +{ + std::vector output; + output.reserve(waypoints.size()); + for (const auto& wp : waypoints) + { + if (wp.graph_index().has_value()) + { + if (*wp.graph_index() != output.back()) + { + output.push_back(*wp.graph_index()); + } + } + } + return output; +} +} // anonymous namespace + +//============================================================================== +bool LockMutexGroup::Active::_consider_plan_result( + services::FindPath::Result result) +{ + if (!result.success()) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Replanning for [%s] after locking mutexes %s because the path to the " + "goal has become blocked.", + _context->requester_id().c_str(), + _data.all_groups_str().c_str()); + return false; + } + + const auto original_sequence = filter_graph_indices(_data.waypoints); + const auto new_sequence = filter_graph_indices(result->get_waypoints()); + if (original_sequence != new_sequence) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Replanning for [%s] after locking mutexes %s because the external " + "traffic has substantially changed.", + _context->requester_id().c_str(), + _data.all_groups_str().c_str()); + return false; + } + + return true; +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp new file mode 100644 index 000000000..1d8c75b35 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2023 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 SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP + +#include "../agv/RobotContext.hpp" +#include "../services/FindPath.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +std::string all_str(const std::unordered_set& all); + +//============================================================================== +class LockMutexGroup : public rmf_task_sequence::Event +{ +public: + struct Data + { + std::unordered_set mutex_groups; + std::string hold_map; + Eigen::Vector3d hold_position; + rmf_traffic::Time hold_time; + std::shared_ptr plan_id; + std::shared_ptr resume_itinerary; + std::vector waypoints; + rmf_traffic::agv::Plan::Goal goal; + + std::string all_groups_str() const; + }; + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + const AssignIDPtr& id, + Data data); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + Standby(Data data); + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + Data _data; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + Data data); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + Active(Data data); + void _initialize(); + void _schedule(rmf_traffic::schedule::Itinerary itinerary) const; + void _apply_cumulative_delay(); + bool _consider_plan_result(services::FindPath::Result result); + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + std::function _finished; + rmf_rxcpp::subscription_guard _listener; + rclcpp::TimerBase::SharedPtr _delay_timer; + std::shared_ptr _stubborn; + Data _data; + std::unordered_set _remaining; + rmf_rxcpp::subscription_guard _plan_subscription; + std::shared_ptr _find_path_service; + rclcpp::TimerBase::SharedPtr _find_path_timeout; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp index cc3eb5371..89d1aecc8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp @@ -223,6 +223,7 @@ void PerformAction::Active::cancel() { _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); + auto self = shared_from_this(); _finished(); if (auto data = _execution_data.lock()) data->okay = false; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp new file mode 100644 index 000000000..8d112de27 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2023 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 "WaitForCancel.hpp" + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +auto WaitForCancel::Standby::make( + agv::RobotContextPtr context, + const AssignIDPtr& id) +-> std::shared_ptr +{ + auto standby = std::shared_ptr(new Standby); + standby->_context = std::move(context); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + "Wait for cancel", + "This task will remain active until it gets canceled", + rmf_task::Event::Status::Standby, {}, standby->_context->clock()); + return standby; +} + +//============================================================================== +auto WaitForCancel::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration WaitForCancel::Standby::duration_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto WaitForCancel::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + return Active::make( + _context, + _state, + std::move(finished)); +} + +//============================================================================== +auto WaitForCancel::Active::make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished) -> std::shared_ptr +{ + auto active = std::shared_ptr(new Active); + active->_context = std::move(context); + active->_finished = std::move(finished); + active->_state = std::move(state); + active->_state->update_status(Status::Underway); + return active; +} + +//============================================================================== +auto WaitForCancel::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration WaitForCancel::Active::remaining_time_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto WaitForCancel::Active::backup() const -> Backup +{ + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto WaitForCancel::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + _context->worker().schedule([task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + return Resume::make([]() { /* do nothing */}); +} + +//============================================================================== +void WaitForCancel::Active::cancel() +{ + _state->update_status(Status::Canceled); + _context->worker().schedule([finished = _finished](const auto&) + { + finished(); + }); +} + +//============================================================================== +void WaitForCancel::Active::kill() +{ + cancel(); +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp new file mode 100644 index 000000000..9c4aad415 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2023 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 SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP + +#include "../agv/RobotContext.hpp" + +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class WaitForCancel : public rmf_task_sequence::Event +{ +public: + class Standby : public rmf_task_sequence::Event::Standby + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + const AssignIDPtr& id); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + Standby() = default; + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + std::function _finished; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp index 1be020b61..f0acae3e4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -23,7 +23,7 @@ namespace events { //============================================================================== auto WaitForTraffic::Standby::make( agv::RobotContextPtr context, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, rmf_traffic::Dependencies dependencies, rmf_traffic::Time expected_time, const AssignIDPtr& id, @@ -59,9 +59,28 @@ auto WaitForTraffic::Standby::begin( std::function, std::function finished) -> ActivePtr { + RCLCPP_INFO( + _context->node()->get_logger(), + "[%s] waiting for traffic", + _context->requester_id().c_str()); + rmf_traffic::PlanId plan_id = 0; + if (_plan_id) + { + plan_id = *_plan_id; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No plan_id was provided for WaitForTraffic action for robot [%s]. This " + "is a critical internal error, please report this bug to the RMF " + "maintainers.", + _context->requester_id().c_str()); + } + return Active::make( _context, - _plan_id, + plan_id, _dependencies, _expected_time, _state, @@ -79,6 +98,9 @@ auto WaitForTraffic::Active::make( std::function update, std::function finished) -> std::shared_ptr { + using MutexGroupRequestPtr = + std::shared_ptr; + auto active = std::make_shared(); active->_context = std::move(context); active->_plan_id = plan_id; @@ -87,6 +109,37 @@ auto WaitForTraffic::Active::make( active->_update = std::move(update); active->_finished = std::move(finished); + active->_mutex_group_listener = active->_context->node() + ->mutex_group_request_obs() + .observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe([w = active->weak_from_this()](const MutexGroupRequestPtr& msg) + { + const auto self = w.lock(); + if (!self) + return; + + if (msg->claimant == self->_context->participant_id()) + { + // We can ignore our own mutex group requests + return; + } + + if (self->_context->locked_mutex_groups().count(msg->group) > 0) + { + // If another participant is waiting to lock a mutex that we have + // already locked, then we must delete any dependencies related to + // that participant. + auto r_it = std::remove_if( + self->_dependencies.begin(), + self->_dependencies.end(), + [&](const DependencySubscription& d) + { + return d.dependency().on_participant == msg->claimant; + }); + self->_dependencies.erase(r_it, self->_dependencies.end()); + } + }); + const auto consider_going = [w = active->weak_from_this()]() { if (const auto self = w.lock()) @@ -221,6 +274,10 @@ void WaitForTraffic::Active::_consider_going() _decision_made = std::chrono::steady_clock::now(); _state->update_status(Status::Completed); _state->update_log().info("All traffic dependencies satisfied"); + RCLCPP_INFO( + _context->node()->get_logger(), + "[%s] done waiting for traffic", + _context->requester_id().c_str()); return _finished(); } @@ -233,6 +290,10 @@ void WaitForTraffic::Active::_consider_going() _state->update_status(Status::Delayed); _state->update_log().info( "Replanning because a traffic dependency is excessively delayed"); + RCLCPP_INFO( + _context->node()->get_logger(), + "Replanning for [%s] because a traffic dependency is excessively delayed", + _context->requester_id().c_str()); return _replan(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp index 56e30605b..d4651c2be 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp @@ -19,6 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORTRAFFIC_HPP #include "../agv/RobotContext.hpp" +#include "../LegacyTask.hpp" #include #include @@ -38,7 +39,7 @@ class WaitForTraffic : public rmf_task_sequence::Event static std::shared_ptr make( agv::RobotContextPtr context, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, rmf_traffic::Dependencies dependencies, rmf_traffic::Time expected_time, const AssignIDPtr& id, @@ -54,7 +55,7 @@ class WaitForTraffic : public rmf_task_sequence::Event private: agv::RobotContextPtr _context; - rmf_traffic::PlanId _plan_id; + PlanIdPtr _plan_id; rmf_traffic::Dependencies _dependencies; rmf_traffic::Time _expected_time; rmf_task::events::SimpleEventStatePtr _state; @@ -104,6 +105,7 @@ class WaitForTraffic : public rmf_task_sequence::Event std::function _finished; rclcpp::TimerBase::SharedPtr _timer; std::optional _decision_made; + rmf_rxcpp::subscription_guard _mutex_group_listener; }; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp index 6f97efb97..a5df28611 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp @@ -23,18 +23,19 @@ namespace phases { //============================================================================== DockRobot::ActivePhase::ActivePhase( agv::RobotContextPtr context, - std::string dock_name) + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + rmf_traffic::PlanId plan_id) : _context{std::move(context)}, _dock_name{std::move(dock_name)}, + _waypoint(std::move(waypoint)), + _plan_id(plan_id), _be_stubborn(_context->be_stubborn()) { std::ostringstream oss; oss << "Docking robot to " << _dock_name; _description = oss.str(); - _action = std::make_shared(this); - _obs = rmf_rxcpp::make_job(_action); - _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_DOCKING); } @@ -42,7 +43,7 @@ DockRobot::ActivePhase::ActivePhase( const rxcpp::observable& DockRobot::ActivePhase::observe() const { - return _obs; + return obs; } //============================================================================== @@ -73,9 +74,13 @@ const std::string& DockRobot::ActivePhase::description() const //============================================================================== DockRobot::PendingPhase::PendingPhase( agv::RobotContextPtr context, - std::string dock_name) + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + PlanIdPtr plan_id) : _context{std::move(context)}, - _dock_name{std::move(dock_name)} + _dock_name{std::move(dock_name)}, + _waypoint(std::move(waypoint)), + _plan_id(std::move(plan_id)) { std::ostringstream oss; oss << "Dock robot to " << _dock_name; @@ -85,7 +90,24 @@ DockRobot::PendingPhase::PendingPhase( //============================================================================== std::shared_ptr DockRobot::PendingPhase::begin() { - return std::make_shared(_context, _dock_name); + rmf_traffic::PlanId plan_id = 0; + if (_plan_id) + { + plan_id = *_plan_id; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No plan_id was provided for MoveRobot action for robot [%s]. This is a " + "critical internal error, please report this bug to the RMF maintainers.", + _context->requester_id().c_str()); + } + auto active = std::make_shared( + _context, _dock_name, _waypoint, plan_id); + active->action = std::make_shared(active->weak_from_this()); + active->obs = rmf_rxcpp::make_job(active->action); + return active; } //============================================================================== @@ -102,7 +124,7 @@ const std::string& DockRobot::PendingPhase::description() const } //============================================================================== -DockRobot::Action::Action(ActivePhase* phase) +DockRobot::Action::Action(std::weak_ptr phase) : _phase(phase) { // Do nothing diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp index f76a9d90b..8bd1113c5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -29,13 +29,16 @@ struct DockRobot { class Action; - class ActivePhase : public LegacyTask::ActivePhase + class ActivePhase : public LegacyTask::ActivePhase, + public std::enable_shared_from_this { public: ActivePhase( agv::RobotContextPtr context, - std::string dock_name); + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + rmf_traffic::PlanId plan_id); const rxcpp::observable& observe() const override; @@ -47,14 +50,16 @@ struct DockRobot const std::string& description() const override; + std::shared_ptr action; + rxcpp::observable obs; private: friend class Action; agv::RobotContextPtr _context; std::string _dock_name; std::string _description; - std::shared_ptr _action; - rxcpp::observable _obs; + rmf_traffic::agv::Plan::Waypoint _waypoint; + rmf_traffic::PlanId _plan_id; std::shared_ptr _be_stubborn; }; @@ -64,7 +69,9 @@ struct DockRobot PendingPhase( agv::RobotContextPtr context, - std::string dock_name); + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + PlanIdPtr plan_id); std::shared_ptr begin() override; @@ -77,19 +84,21 @@ struct DockRobot agv::RobotContextPtr _context; std::string _dock_name; std::string _description; + rmf_traffic::agv::Plan::Waypoint _waypoint; + PlanIdPtr _plan_id; }; class Action { public: - Action(ActivePhase* phase); + Action(std::weak_ptr phase); template void operator()(const Subscriber& s); private: - ActivePhase* _phase; + std::weak_ptr _phase; }; }; @@ -97,23 +106,57 @@ struct DockRobot template void DockRobot::Action::operator()(const Subscriber& s) { - LegacyTask::StatusMsg status; - status.state = LegacyTask::StatusMsg::STATE_ACTIVE; - status.status = "Docking [" + _phase->_context->requester_id() + - "] into dock [" - + _phase->_dock_name + "]"; - - s.on_next(status); - _phase->_context->command()->dock( - _phase->_dock_name, - [s, dock_name = _phase->_dock_name, context = _phase->_context]() + const auto active = _phase.lock(); + if (!active) + return; + + active->_context->worker().schedule( + [s, w = active->weak_from_this()](const auto&) { + const auto active = w.lock(); + if (!active) + return; + LegacyTask::StatusMsg status; - status.status = "Finished docking [" + context->requester_id() - + "] into dock [" + dock_name + "]"; - status.state = LegacyTask::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_ACTIVE; + status.status = "Docking [" + active->_context->requester_id() + + "] into dock [" + + active->_dock_name + "]"; + s.on_next(status); - s.on_completed(); + active->_context->command()->dock( + active->_dock_name, + [s, dock_name = active->_dock_name, context = active->_context, + wp = active->_waypoint, plan_id = active->_plan_id]() + { + context->worker().schedule( + [s, dock_name, context, wp, plan_id](const auto&) + { + LegacyTask::StatusMsg status; + status.status = "Finished docking [" + context->requester_id() + + "] into dock [" + dock_name + "]"; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; + for (const auto& c : wp.arrival_checkpoints()) + { + context->itinerary().reached(plan_id, c.route_id, + c.checkpoint_id); + } + + if (wp.graph_index().has_value()) + { + const auto& graph = context->navigation_graph(); + context->retain_mutex_groups( + {graph.get_waypoint(*wp.graph_index()).in_mutex_group()}); + } + + const auto now = context->now(); + const auto cumulative_delay = now - wp.time(); + context->itinerary().cumulative_delay( + plan_id, cumulative_delay, std::chrono::seconds(1)); + s.on_next(status); + s.on_completed(); + }); + }); }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp index 0ccb1b678..6a8552737 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp @@ -29,6 +29,12 @@ std::shared_ptr DoorClose::ActivePhase::make( std::string door_name, std::string request_id) { + RCLCPP_INFO( + context->node()->get_logger(), + "Releasing door [%s] for [%s]", + door_name.c_str(), + context->requester_id().c_str()); + context->_release_door(door_name); auto inst = std::shared_ptr(new ActivePhase( std::move(context), std::move(door_name), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp index 3d5f9d82b..899d7a12c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp @@ -54,7 +54,13 @@ DoorOpen::ActivePhase::ActivePhase( _request_id(std::move(request_id)), _expected_finish(std::move(expected_finish)) { + _context->_hold_door(_door_name); _description = "Opening [door:" + _door_name + "]"; + RCLCPP_INFO( + _context->node()->get_logger(), + "Opening door [%s] for [%s]", + _door_name.c_str(), + _context->requester_id().c_str()); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp index a4224e938..a1c85a2ea 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp @@ -45,7 +45,13 @@ EndLiftSession::Active::Active( _lift_name(std::move(lift_name)), _destination(std::move(destination)) { - _description = "Ending session with lift [" + lift_name + "]"; + _description = "Ending session with lift [" + _lift_name + "]"; + RCLCPP_INFO( + _context->node()->get_logger(), + "Ending lift [%s] session for [%s]", + _lift_name.c_str(), + _context->requester_id().c_str()); + _context->release_lift(); } //============================================================================== @@ -86,24 +92,6 @@ void EndLiftSession::Active::_init_obs() using rmf_lift_msgs::msg::LiftRequest; using rmf_lift_msgs::msg::LiftState; _obs = _context->node()->lift_state() - .lift(on_subscribe([weak = weak_from_this()]() - { - const auto me = weak.lock(); - if (!me) - return; - - me->_publish_session_end(); - me->_timer = me->_context->node()->try_create_wall_timer( - std::chrono::milliseconds(1000), - [weak]() - { - const auto me = weak.lock(); - if (!me) - return; - - me->_publish_session_end(); - }); - })) .map([weak = weak_from_this()](const LiftState::SharedPtr& state) { const auto me = weak.lock(); @@ -124,27 +112,7 @@ void EndLiftSession::Active::_init_obs() return msg; }) - .lift(grab_while_active()) - .finally([weak = weak_from_this()]() - { - const auto me = weak.lock(); - if (!me) - return; - - me->_timer.reset(); - }); -} - -//============================================================================== -void EndLiftSession::Active::_publish_session_end() -{ - rmf_lift_msgs::msg::LiftRequest msg; - msg.lift_name = _lift_name; - msg.destination_floor = _destination; - msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; - msg.session_id = _context->requester_id(); - - _context->node()->lift_request()->publish(msg); + .lift(grab_while_active()); } //============================================================================== @@ -156,7 +124,7 @@ EndLiftSession::Pending::Pending( _lift_name(std::move(lift_name)), _destination(std::move(destination)) { - _description = "End session with lift [" + lift_name + "]"; + _description = "End session with lift [" + _lift_name + "]"; } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp index fdd2e339b..dc15dc0de 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp @@ -59,10 +59,8 @@ struct EndLiftSession std::string _destination; std::string _description; rxcpp::observable _obs; - rclcpp::TimerBase::SharedPtr _timer; void _init_obs(); - void _publish_session_end(); }; class Pending : public LegacyTask::PendingPhase diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp index 2f9479aa1..92208d4fe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -83,7 +83,7 @@ const std::string& MoveRobot::ActivePhase::description() const MoveRobot::PendingPhase::PendingPhase( agv::RobotContextPtr context, std::vector waypoints, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, std::optional tail_period) : _context{std::move(context)}, _waypoints{std::move(waypoints)}, @@ -93,15 +93,29 @@ MoveRobot::PendingPhase::PendingPhase( std::ostringstream oss; const auto dest = destination( _waypoints.back(), _context->planner()->get_configuration().graph()); - oss << "Move to " << dest; + oss << "Move to " << dest << " <" << _waypoints.back().position().transpose() + << "> through " << _waypoints.size() << " points"; _description = oss.str(); } //============================================================================== std::shared_ptr MoveRobot::PendingPhase::begin() { + rmf_traffic::PlanId plan_id = 0; + if (_plan_id) + { + plan_id = *_plan_id; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No plan_id was provided for MoveRobot action for robot [%s]. This is a " + "critical internal error, please report this bug to the RMF maintainers.", + _context->requester_id().c_str()); + } return std::make_shared( - _context, _waypoints, _plan_id, _tail_period); + _context, _waypoints, plan_id, _tail_period); } //============================================================================== @@ -128,7 +142,16 @@ MoveRobot::Action::Action( _plan_id{plan_id}, _tail_period{tail_period} { - // no op + _first_graph_index = [&]() -> std::optional + { + for (const auto& wp : _waypoints) + { + if (wp.graph_index().has_value()) + return wp.graph_index(); + } + + return std::nullopt; + }(); } } // namespace phases diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp index 64f3fd5f5..575730fcd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -84,7 +84,7 @@ struct MoveRobot PendingPhase( agv::RobotContextPtr context, std::vector waypoints, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, std::optional tail_period); std::shared_ptr begin() override; @@ -97,7 +97,7 @@ struct MoveRobot agv::RobotContextPtr _context; std::vector _waypoints; - rmf_traffic::PlanId _plan_id; + PlanIdPtr _plan_id; std::optional _tail_period; std::string _description; }; @@ -123,6 +123,7 @@ struct MoveRobot std::optional _tail_period; std::optional _last_tail_bump; std::size_t _next_path_index = 0; + std::optional _first_graph_index; rclcpp::TimerBase::SharedPtr _update_timeout_timer; rclcpp::Time _last_update_rostime; @@ -138,164 +139,250 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (!command) return; - _last_update_rostime = _context->node()->now(); - _update_timeout_timer = _context->node()->try_create_wall_timer( - _update_timeout, [w = weak_from_this()]() + _context->worker().schedule([w = weak_from_this(), s](const auto&) { const auto self = w.lock(); if (!self) return; - const auto now = self->_context->node()->now(); - if (now < self->_last_update_rostime + self->_update_timeout) - { - // The simulation is paused or running slowly, so we should allow more - // patience before assuming that there's been a timeout. - return; - } - - self->_last_update_rostime = now; - - // The RobotCommandHandle seems to have frozen up. Perhaps a bug in the - // user's code has caused the RobotCommandHandle to drop the command. We - // will request a replan. - RCLCPP_WARN( - self->_context->node()->get_logger(), - "Requesting replan for [%s] because its command handle seems to be " - "unresponsive", - self->_context->requester_id().c_str()); - self->_context->request_replan(); - }); + self->_last_update_rostime = self->_context->node()->now(); + self->_update_timeout_timer = self->_context->node()->try_create_wall_timer( + self->_update_timeout, + [w = self->weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; - _context->command()->follow_new_path( - _waypoints, - [s, w_action = weak_from_this(), r = _context->requester_id()]( - std::size_t path_index, rmf_traffic::Duration estimate) - { - const auto action = w_action.lock(); - if (!action) - return; + const auto now = self->_context->node()->now(); + if (now < self->_last_update_rostime + self->_update_timeout) + { + // The simulation is paused or running slowly, so we should allow more + // patience before assuming that there's been a timeout. + return; + } - action->_last_update_rostime = action->_context->node()->now(); - action->_update_timeout_timer->reset(); + self->_last_update_rostime = now; + + // The RobotCommandHandle seems to have frozen up. Perhaps a bug in the + // user's code has caused the RobotCommandHandle to drop the command. We + // will request a replan. + RCLCPP_WARN( + self->_context->node()->get_logger(), + "Requesting replan for [%s] because its command handle seems to be " + "unresponsive", + self->_context->requester_id().c_str()); + self->_context->request_replan(); + }); - if (path_index == action->_waypoints.size()-1 - && estimate < std::chrono::seconds(1) - && action->_tail_period.has_value()) + const auto update = [ + s, + w_action = self->weak_from_this(), + r = self->_context->requester_id() + ]( + std::size_t path_index, rmf_traffic::Duration estimate) { - const auto now = action->_context->now(); - if (!action->_last_tail_bump.has_value() - || *action->_last_tail_bump + *action->_tail_period < now) + const auto action = w_action.lock(); + if (!action) + return; + + action->_last_update_rostime = action->_context->node()->now(); + action->_update_timeout_timer->reset(); + + if (path_index == action->_waypoints.size()-1 + && estimate < std::chrono::seconds(1) + && action->_tail_period.has_value()) { - action->_last_tail_bump = now; - action->_context->worker().schedule( - [ - context = action->_context, - bump = *action->_tail_period, - plan_id = action->_plan_id - ]( - const auto&) - { - if (const auto c = context->itinerary().cumulative_delay(plan_id)) + const auto now = action->_context->now(); + if (!action->_last_tail_bump.has_value() + || *action->_last_tail_bump + *action->_tail_period < now) + { + action->_last_tail_bump = now; + action->_context->worker().schedule( + [ + context = action->_context, + bump = *action->_tail_period, + plan_id = action->_plan_id + ]( + const auto&) { - context->itinerary().cumulative_delay(plan_id, *c + bump); - } - }); + if (const auto c = + context->itinerary().cumulative_delay(plan_id)) + { + context->itinerary().cumulative_delay(plan_id, *c + bump); + } + }); + } } - } - if (path_index != action->_next_path_index) - { - action->_next_path_index = path_index; - LegacyTask::StatusMsg msg; - msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; - - if (path_index < action->_waypoints.size()) + if (path_index != action->_next_path_index) { - msg.status = "Heading towards " - + destination( - action->_waypoints[path_index], - action->_context->planner()->get_configuration().graph()); + action->_next_path_index = path_index; + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; + + if (path_index < action->_waypoints.size()) + { + msg.status = "Heading towards " + + destination( + action->_waypoints[path_index], + action->_context->planner()->get_configuration().graph()); + } + else + { + // TODO(MXG): This should really be a warning, but the legacy phase shim + // does not have a way for us to specify a warning. + msg.status = "[Bug] [MoveRobot] Current path index was specified as [" + + std::to_string(path_index) + "] but that exceeds the limit of [" + + std::to_string(action->_waypoints.size()-1) + "]"; + } + + s.on_next(msg); } - else + + if (action->_next_path_index > action->_waypoints.size()) { - // TODO(MXG): This should really be a warning, but the legacy phase shim - // does not have a way for us to specify a warning. - msg.status = "[Bug] [MoveRobot] Current path index was specified as [" - + std::to_string(path_index) + "] but that exceeds the limit of [" - + std::to_string(action->_waypoints.size()-1) + "]"; + return; } - s.on_next(msg); - } - - if (action->_next_path_index > action->_waypoints.size()) - { - return; - } - - if (action->_plan_id != action->_context->itinerary().current_plan_id()) - { - // If the current Plan ID of the itinerary does not match the Plan ID - // of this action, then we should not modify the delay here. - return; - } - - const auto& target_wp = action->_waypoints[path_index]; - using namespace std::chrono_literals; - const rmf_traffic::Time now = action->_context->now(); - const auto planned_time = target_wp.time(); - const auto newly_expected_arrival = now + estimate; - const auto new_cumulative_delay = newly_expected_arrival - planned_time; - action->_context->worker().schedule( - [ - context = action->_context, - plan_id = action->_plan_id, - now, - new_cumulative_delay - ](const auto&) + if (action->_plan_id != action->_context->itinerary().current_plan_id()) { - context->itinerary().cumulative_delay( - plan_id, new_cumulative_delay, 100ms); + // If the current Plan ID of the itinerary does not match the Plan ID + // of this action, then we should not modify the delay here. + return; + } - const auto& itin = context->itinerary().itinerary(); - for (std::size_t i = 0; i < itin.size(); ++i) + const auto& target_wp = action->_waypoints[path_index]; + using namespace std::chrono_literals; + const rmf_traffic::Time now = action->_context->now(); + const auto planned_time = target_wp.time(); + const auto newly_expected_arrival = now + estimate; + const auto new_cumulative_delay = newly_expected_arrival - planned_time; + + action->_context->worker().schedule( + [ + w = action->weak_from_this(), + now, + new_cumulative_delay + ](const auto&) { - const auto& traj = itin[i].trajectory(); - const auto t_it = traj.find(now); - if (t_it != traj.end() && t_it != traj.begin()) + const auto self = w.lock(); + if (!self) + return; + + const auto context = self->_context; + const auto plan_id = self->_plan_id; + context->itinerary().cumulative_delay( + plan_id, new_cumulative_delay, 100ms); + + // This itinerary has been adjusted according to the latest delay + // information, so our position along the trajectory is given by `now` + const auto& itin = context->itinerary().itinerary(); + for (std::size_t i = 0; i < itin.size(); ++i) { - if (t_it->time() == now) + const auto& traj = itin[i].trajectory(); + const auto t_it = traj.find(now); + if (t_it != traj.end() && t_it != traj.begin()) { - context->itinerary().reached(plan_id, i, t_it->index()); + std::size_t index = t_it->index() - 1; + if (t_it->time() == now) + { + index = t_it->index(); + } + + context->itinerary().reached(plan_id, i, index); } - else + } + + if (!context->locked_mutex_groups().empty()) + { + const auto adjusted_now = now - new_cumulative_delay; + const auto& graph = context->navigation_graph(); + std::unordered_set retain_mutexes; + for (const auto& wp : self->_waypoints) { - context->itinerary().reached(plan_id, i, t_it->index() - 1); + const auto s_100 = + (int)(rmf_traffic::time::to_seconds(adjusted_now - + wp.time()) * 100); + const auto s = (double)(s_100)/100.0; + if (wp.time() < adjusted_now) + { + continue; + } + + if (wp.graph_index().has_value()) + { + retain_mutexes.insert( + graph.get_waypoint(*wp.graph_index()).in_mutex_group()); + } + + for (const auto& l : wp.approach_lanes()) + { + retain_mutexes.insert( + graph.get_lane(l).properties().in_mutex_group()); + } } + + context->retain_mutex_groups(retain_mutexes); } - } - }); - }, - [s, w = weak_from_this()]() - { - if (const auto self = w.lock()) + }); + }; + + const auto finish = [ + s, + w = self->weak_from_this(), + name = self->_context->requester_id() + ]() { - if (!self->_waypoints.empty()) + if (const auto self = w.lock()) { - for (const auto& c : self->_waypoints.back().arrival_checkpoints()) + if (!self->_waypoints.empty()) { - self->_context->itinerary().reached( - self->_plan_id, c.route_id, c.checkpoint_id); + for (const auto& c : self->_waypoints.back().arrival_checkpoints()) + { + self->_context->itinerary().reached( + self->_plan_id, c.route_id, c.checkpoint_id); + } + + const auto last_index = self->_waypoints.back().graph_index(); + if (last_index.has_value()) + { + const auto& graph = self->_context->navigation_graph(); + self->_context->retain_mutex_groups( + {graph.get_waypoint(*last_index).in_mutex_group()}); + } + + const auto now = self->_context->now(); + const auto cumulative_delay = now - self->_waypoints.back().time(); + self->_context->itinerary().cumulative_delay( + self->_plan_id, cumulative_delay, std::chrono::seconds(1)); } + + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; + msg.status = "move robot success"; + s.on_next(msg); + s.on_completed(); } + }; - LegacyTask::StatusMsg msg; - msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; - msg.status = "move robot success"; - s.on_next(msg); - s.on_completed(); - } + self->_context->command()->follow_new_path( + self->_waypoints, + [worker = self->_context->worker(), update]( + std::size_t path_index, rmf_traffic::Duration estimate) + { + worker.schedule([path_index, estimate, update](const auto&) + { + update(path_index, estimate); + }); + }, + [worker = self->_context->worker(), finish]() + { + worker.schedule([finish](const auto&) + { + finish(); + }); + }); }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index bc76dc870..57272a2db 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -18,6 +18,8 @@ #include "RequestLift.hpp" #include "EndLiftSession.hpp" #include "RxOperators.hpp" +#include "../agv/internal_RobotUpdateHandle.hpp" +#include "../agv/internal_EasyFullControl.hpp" namespace rmf_fleet_adapter { namespace phases { @@ -27,17 +29,16 @@ std::shared_ptr RequestLift::ActivePhase::make( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - const Located located) + Data data) { auto inst = std::shared_ptr( new ActivePhase( std::move(context), std::move(lift_name), std::move(destination), - std::move(expected_finish), - located + std::move(data) )); + inst->_init_obs(); return inst; } @@ -65,7 +66,7 @@ void RequestLift::ActivePhase::emergency_alarm(bool /*on*/) //============================================================================== void RequestLift::ActivePhase::cancel() { - _cancelled.get_subscriber().on_next(true); + // GoToPlace and ExecutePlan don't call the cancel function anyway } //============================================================================== @@ -79,13 +80,11 @@ RequestLift::ActivePhase::ActivePhase( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located) + Data data) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), - _expected_finish(std::move(expected_finish)), - _located(located) + _data(std::move(data)) { std::ostringstream oss; oss << "Requesting lift [" << lift_name << "] to [" << destination << "]"; @@ -98,7 +97,49 @@ void RequestLift::ActivePhase::_init_obs() { using rmf_lift_msgs::msg::LiftState; + if (_data.located == Located::Outside && _context->current_lift_destination()) + { + // Check if the current destination is the one we want and also has arrived. + // If so, we can skip the rest of this process and just make an observable + // that says it's completed right away. + if (_context->current_lift_destination()->matches(_lift_name, _destination)) + { + _obs = rxcpp::observable<>::create( + [w = weak_from_this()](rxcpp::subscriber s) + { + const auto self = w.lock(); + if (!self) + return; + + if (self->_data.resume_itinerary) + { + self->_context->schedule_itinerary( + self->_data.plan_id, *self->_data.resume_itinerary); + const auto delay = + self->_context->now() - self->_data.expected_finish; + self->_context->itinerary().cumulative_delay( + *self->_data.plan_id, delay); + } + + s.on_completed(); + }); + return; + } + } + + if (_data.hold_point.has_value()) + { + *_data.plan_id = _context->itinerary().assign_plan_id(); + _context->schedule_hold( + _data.plan_id, + _data.hold_point->time(), + std::chrono::seconds(10), + _data.hold_point->position(), + _destination); + } + _obs = _context->node()->lift_state() + .observe_on(rxcpp::identity_same_worker(_context->worker())) .lift( on_subscribe( [weak = weak_from_this()]() @@ -119,17 +160,17 @@ void RequestLift::ActivePhase::_init_obs() // TODO(MXG): We can stop publishing the door request once the // supervisor sees our request. me->_do_publish(); - - const auto current_expected_finish = - me->_expected_finish + me->_context->itinerary().delay(); - - const auto delay = me->_context->now() - current_expected_finish; + const auto delay = me->_context->now() - me->_data.expected_finish; if (delay > std::chrono::seconds(0)) { me->_context->worker().schedule( - [context = me->_context, delay](const auto&) + [ + context = me->_context, + plan_id = *me->_data.plan_id, + delay + ](const auto&) { - context->itinerary().delay(delay); + context->itinerary().cumulative_delay(plan_id, delay); }); } }); @@ -166,21 +207,62 @@ void RequestLift::ActivePhase::_init_obs() if (!me) return; - // FIXME: is this thread-safe? - if (!me->_cancelled.get_value() || me->_located == Located::Inside) + if (me->_data.localize_after.has_value()) { - s.on_completed(); - } - else if (me->_located == Located::Outside) - { - auto transport = me->_context->node(); - me->_lift_end_phase = EndLiftSession::Active::make( + auto finish = [s, worker = me->_context->worker(), weak]() + { + worker.schedule([s, weak](const auto&) + { + if (const auto me = weak.lock()) + { + if (!me->_finish()) + { + return; + } + } + + s.on_completed(); + }); + }; + + auto cmd = agv::EasyFullControl + ::CommandExecution::Implementation::make_hold( me->_context, - me->_lift_name, - me->_destination); + me->_data.expected_finish, + *me->_data.plan_id, + std::move(finish)); - me->_lift_end_phase->observe().subscribe(s); + agv::Destination::Implementation::get(*me->_data.localize_after) + .position = me->_context->position(); + + if (me->_context->localize(*me->_data.localize_after, + std::move(cmd))) + { + me->_rewait_timer = me->_context->node()->try_create_wall_timer( + std::chrono::seconds(300), + [weak, s] + { + const auto me = weak.lock(); + if (!me) + return; + + RCLCPP_ERROR( + me->_context->node()->get_logger(), + "Waiting for robot [%s] to localize timed out. Please " + "ensure that your localization function triggers " + "execution.finished() when the robot's localization " + "process is finished.", + me->_context->requester_id().c_str()); + + if (me->_finish()) + s.on_completed(); + }); + return; + } } + + if (me->_finish()) + s.on_completed(); })); } @@ -198,6 +280,11 @@ LegacyTask::StatusMsg RequestLift::ActivePhase::_get_status( lift_state->door_state == LiftState::DOOR_OPEN && lift_state->session_id == _context->requester_id()) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Lift has arrived on floor [%s] and opened its doors for robot [%s]", + lift_state->current_floor.c_str(), + lift_state->session_id.c_str()); bool completed = false; const auto& watchdog = _context->get_lift_watchdog(); @@ -254,7 +341,7 @@ LegacyTask::StatusMsg RequestLift::ActivePhase::_get_status( _watchdog_info.reset(); } } - else if (_located == Located::Outside && watchdog) + else if (_data.located == Located::Outside && watchdog) { _watchdog_info = std::make_shared(); watchdog( @@ -303,15 +390,57 @@ void RequestLift::ActivePhase::_do_publish() if (_rewaiting) return; - rmf_lift_msgs::msg::LiftRequest msg{}; - msg.lift_name = _lift_name; - msg.destination_floor = _destination; - msg.session_id = _context->requester_id(); - msg.request_time = _context->node()->now(); - msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_AGV_MODE; - msg.door_state = rmf_lift_msgs::msg::LiftRequest::DOOR_OPEN; + if (!_destination_handle) + { + _destination_handle = _context->set_lift_destination( + _lift_name, _destination, _data.located == Located::Inside); + } +} + +//============================================================================== +bool RequestLift::ActivePhase::_finish() +{ + // The return value of _finish tells us whether we should have the observable + // proceed to trigger on_completed(). If we have already finished before then + // _finished will be true, so we should return false to indicate that the + // observable should not proceed to trigger on_completed(). + if (_finished) + return false; + + _finished = true; + + if (_data.located == Located::Outside) + { + // The robot is going to start moving into the lift now, so we should lock + // the destination in. + _context->set_lift_destination(_lift_name, _destination, true); + + // We should replan to make sure there are no traffic issues that came up + // in the time that we were waiting for the lift. + if (_data.hold_point.has_value()) + { + if (_data.hold_point->graph_index().has_value()) + { + auto start = rmf_traffic::agv::Plan::Start( + _context->now(), + _data.hold_point->graph_index().value(), + _data.hold_point->position()[2]); + _context->set_location({std::move(start)}); + } + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Requesting replan for [%s] because it has finished waiting lift [%s] " + "to arrive at [%s]", + _context->requester_id().c_str(), + _lift_name.c_str(), + _destination.c_str()); + _context->request_replan(); + return false; + } - _context->node()->lift_request()->publish(msg); + return true; } //============================================================================== @@ -319,13 +448,11 @@ RequestLift::PendingPhase::PendingPhase( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located) + Data data) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), - _expected_finish(std::move(expected_finish)), - _located(located) + _data(std::move(data)) { std::ostringstream oss; oss << "Requesting lift \"" << lift_name << "\" to \"" << destination << "\""; @@ -340,8 +467,7 @@ std::shared_ptr RequestLift::PendingPhase::begin() _context, _lift_name, _destination, - _expected_finish, - _located); + _data); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp index 6197ee306..cc6b9233d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -34,6 +34,17 @@ struct RequestLift Outside }; + struct Data + { + rmf_traffic::Time expected_finish; + Located located; + PlanIdPtr plan_id; + std::optional localize_after = std::nullopt; + std::shared_ptr resume_itinerary = + nullptr; + std::optional hold_point = std::nullopt; + }; + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { @@ -43,8 +54,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located); + Data data); const rxcpp::observable& observe() const override; @@ -61,15 +71,16 @@ struct RequestLift agv::RobotContextPtr _context; std::string _lift_name; std::string _destination; - rmf_traffic::Time _expected_finish; + Data _data; rxcpp::subjects::behavior _cancelled = rxcpp::subjects::behavior(false); std::string _description; rxcpp::observable _obs; rclcpp::TimerBase::SharedPtr _timer; std::shared_ptr _lift_end_phase; - Located _located; rmf_rxcpp::subscription_guard _reset_session_subscription; + std::shared_ptr _destination_handle; + bool _finished = false; struct WatchdogInfo { @@ -85,8 +96,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located); + Data data); void _init_obs(); @@ -94,6 +104,7 @@ struct RequestLift const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state); void _do_publish(); + bool _finish(); }; class PendingPhase : public LegacyTask::PendingPhase @@ -104,8 +115,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located); + Data data); std::shared_ptr begin() override; @@ -123,8 +133,8 @@ struct RequestLift std::string _lift_name; std::string _destination; rmf_traffic::Time _expected_finish; - Located _located; std::string _description; + Data _data; }; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index b19b4206d..cd48937c2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -32,8 +32,9 @@ rmf_traffic::Duration WaitForCharge::Active::estimate_remaining_time() const { const double capacity = _battery_system.capacity(); const double charging_current = _battery_system.charging_current(); + const double charge_to_soc = _charge_to_soc.value_or(1.0); const double time_estimate = - 3600.0 * capacity * (_charge_to_soc - _context->current_battery_soc()) / + 3600.0 * capacity * (charge_to_soc - _context->current_battery_soc()) / charging_current; return rmf_traffic::time::from_seconds(time_estimate); @@ -48,7 +49,7 @@ void WaitForCharge::Active::emergency_alarm(const bool) //============================================================================== void WaitForCharge::Active::cancel() { - // TODO + _status_publisher.get_subscriber().on_completed(); } //============================================================================== @@ -61,7 +62,7 @@ const std::string& WaitForCharge::Active::description() const WaitForCharge::Active::Active( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, rmf_traffic::Time start_time) : _context(std::move(context)), _battery_system(battery_system), @@ -75,7 +76,12 @@ WaitForCharge::Active::Active( _battery_system.charging_current())); _description = "Charging [" + _context->requester_id() + "] to [" - + std::to_string(100.0 * _charge_to_soc) + "]"; + + std::to_string(100.0 * _charge_to_soc.value_or(1.0)) + "]"; + + RCLCPP_INFO( + _context->node()->get_logger(), + "%s", + _description.c_str()); StatusMsg initial_msg; initial_msg.status = _description; @@ -105,7 +111,7 @@ std::shared_ptr WaitForCharge::Pending::begin() "Robot [%s] has begun waiting for its battery to charge to %.1f%%. " "Please ensure that the robot is charging.", _context->name().c_str(), - _charge_to_soc * 100.0); + _charge_to_soc.value_or(1.0) * 100.0); active->_battery_soc_subscription = _context->observe_battery_soc() .observe_on(rxcpp::identity_same_worker(_context->worker())) @@ -117,9 +123,12 @@ std::shared_ptr WaitForCharge::Pending::begin() if (!active) return; - if (active->_charge_to_soc <= battery_soc) + if (active->_charge_to_soc.has_value()) { - active->_status_publisher.get_subscriber().on_completed(); + if (*active->_charge_to_soc <= battery_soc) + { + active->_status_publisher.get_subscriber().on_completed(); + } } const auto& now = std::chrono::steady_clock::now(); @@ -139,14 +148,13 @@ std::shared_ptr WaitForCharge::Pending::begin() "is %.1f %%/hour. If the battery percentage has not been rising, " "please check that the robot is connected to its charger.", active->_context->name().c_str(), - active->_charge_to_soc * 100.0, + active->_charge_to_soc.value_or(1.0) * 100.0, battery_soc * 100, average_charging_rate, active->_expected_charging_rate); active->_last_update_time = now; } - }); return active; @@ -168,7 +176,7 @@ const std::string& WaitForCharge::Pending::description() const WaitForCharge::Pending::Pending( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, double time_estimate) : _context(std::move(context)), _battery_system(battery_system), @@ -176,20 +184,21 @@ WaitForCharge::Pending::Pending( _time_estimate(time_estimate) { _description = - "Charging robot to [" + std::to_string(100.0 * charge_to_soc) + "%]"; + "Charging robot to [" + std::to_string(100.0 * charge_to_soc.value_or(1.0)) + + "%]"; } //============================================================================== auto WaitForCharge::make( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc) -> std::unique_ptr + std::optional charge_to_soc) -> std::unique_ptr { - const double capacity = battery_system.capacity(); const double charging_current = battery_system.charging_current(); const double time_estimate = - 3600.0 * capacity * (charge_to_soc - context->current_battery_soc()) / + 3600.0 * capacity + * (charge_to_soc.value_or(1.0) - context->current_battery_soc()) / charging_current; return std::unique_ptr( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp index b603fcd92..b61389366 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -62,12 +62,12 @@ class WaitForCharge Active( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, rmf_traffic::Time start_time); agv::RobotContextPtr _context; rmf_battery::agv::BatterySystem _battery_system; - double _charge_to_soc; + std::optional _charge_to_soc; std::string _description; rxcpp::observable _status_obs; rxcpp::subjects::subject _status_publisher; @@ -96,12 +96,12 @@ class WaitForCharge Pending( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, double time_estimate); agv::RobotContextPtr _context; rmf_battery::agv::BatterySystem _battery_system; - double _charge_to_soc; + std::optional _charge_to_soc; std::string _description; double _time_estimate; }; @@ -109,7 +109,7 @@ class WaitForCharge static std::unique_ptr make( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc); + std::optional charge_to_soc); }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 06c5a02bb..863704d0b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -16,7 +16,7 @@ */ #include "../phases/WaitForCharge.hpp" - +#include "../events/WaitForCancel.hpp" #include "../events/GoToPlace.hpp" #include "../events/LegacyPhaseShim.hpp" @@ -28,15 +28,476 @@ #include +#include + +#include namespace rmf_fleet_adapter { namespace tasks { +//============================================================================== +rmf_traffic::Duration estimate_charge_time( + const double initial_soc, + const double recharged_soc, + const rmf_battery::agv::BatterySystem& battery_system) +{ + if (initial_soc < recharged_soc) + { + const double delta_soc = recharged_soc - initial_soc; + const double dt = (3600 * delta_soc * battery_system.capacity()) / + battery_system.charging_current(); + return rmf_traffic::time::from_seconds(dt); + } + + return rmf_traffic::Duration(0); +} + +//============================================================================== +// TODO(MXG): Consider making the ChargeBatteryEvent public so it +// can be incorporated into other task types +class ChargeBatteryEvent : public rmf_task_sequence::Event +{ +public: + + class Model : public rmf_task_sequence::Activity::Model + { + public: + static rmf_task_sequence::Activity::ConstModelPtr make( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) + { + const auto model = std::shared_ptr( + new Model(invariant_initial_state, parameters)); + model->_invariant_initial_state = invariant_initial_state; + + const auto wp_opt = invariant_initial_state.dedicated_charging_waypoint(); + if (wp_opt.has_value()) + { + const auto go_to_place_desc = + rmf_task_sequence::events::GoToPlace::Description::make(*wp_opt); + model->_go_to_place = go_to_place_desc->make_model( + invariant_initial_state, parameters); + } + + model->_parameters = parameters; + return model; + } + + std::optional estimate_finish( + rmf_task::State state, + rmf_traffic::Time earliest_arrival_time, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const final + { + rmf_traffic::Time wait_until = earliest_arrival_time; + if (_go_to_place) + { + const auto estimate_go_to = _go_to_place->estimate_finish( + std::move(state), earliest_arrival_time, constraints, + travel_estimator); + if (!estimate_go_to.has_value()) + return std::nullopt; + + wait_until = estimate_go_to->wait_until(); + state = estimate_go_to->finish_state(); + } + + const auto recharged_soc = constraints.recharge_soc(); + rmf_traffic::Duration dt = rmf_traffic::Duration(0); + const auto initial_soc_opt = state.battery_soc(); + if (!initial_soc_opt.has_value()) + { + // Assume no charging is needed if the "robot" does not have a battery + return rmf_task::Estimate(state, wait_until); + } + + const double initial_soc = *initial_soc_opt; + if (initial_soc < recharged_soc) + { + state.battery_soc(recharged_soc); + } + wait_until += estimate_charge_time( + initial_soc, + recharged_soc, + _parameters.battery_system()); + + return rmf_task::Estimate(state, wait_until); + } + + rmf_traffic::Duration invariant_duration() const final + { + return rmf_traffic::Duration(0); + } + + rmf_task::State invariant_finish_state() const final + { + return _invariant_initial_state; + } + + private: + Model( + rmf_task::State invariant_initial_state, + rmf_task::Parameters parameters) + : _invariant_initial_state(std::move(invariant_initial_state)), + _parameters(std::move(parameters)) + { + // Do nothing + } + rmf_task::State _invariant_initial_state; + rmf_task_sequence::Activity::ConstModelPtr _go_to_place; + rmf_task::Parameters _parameters; + }; + + class Description : public rmf_task_sequence::Activity::Description + { + public: + Description( + std::optional specific_location_, + bool indefinite_, + bool park_) + : specific_location(specific_location_), + indefinite(indefinite_), + park(park_) + { + // Do nothing + } + + std::optional specific_location; + bool indefinite; + bool park; + + rmf_task_sequence::Activity::ConstModelPtr make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const final + { + return Model::make(std::move(invariant_initial_state), parameters); + } + + rmf_task::Header generate_header( + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const final + { + rmf_traffic::Duration duration_estimate = rmf_traffic::Duration(0); + double recharged_soc = 1.0; + if (const auto c = initial_state.get()) + { + if (const auto context = c->value) + { + recharged_soc = context->task_planner() + ->configuration().constraints().recharge_soc(); + + const auto header_go_to = + rmf_task_sequence::events::GoToPlace::Description::make( + context->dedicated_charging_wp())->generate_header( + initial_state, parameters); + duration_estimate += header_go_to.original_duration_estimate(); + } + } + + double initial_soc = initial_state.battery_soc().value_or(0.0); + duration_estimate += estimate_charge_time( + initial_soc, recharged_soc, parameters.battery_system()); + + std::string name = park ? "Park" : "Charge Battery"; + + return rmf_task::Header(name, "", duration_estimate); + } + }; + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + static std::shared_ptr make( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const Description& description, + std::function update) + { + const auto state = get_state(); + const auto context = state.get()->value; + const auto header = description.generate_header(state, *parameters); + + auto standby = std::shared_ptr(new Standby(description)); + standby->_assign_id = id; + standby->_context = context; + standby->_time_estimate = header.original_duration_estimate(); + standby->_update = std::move(update); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + header.category(), + header.detail(), + rmf_task::Event::Status::Standby, + {}, + context->clock()); + + return standby; + } + + ConstStatePtr state() const final + { + return _state; + } + + rmf_traffic::Duration duration_estimate() const final + { + return _time_estimate; + } + + ActivePtr begin( + std::function, + std::function finished) final + { + if (!_active) + { + const std::string& task = _desc.park ? "parking" : "charging"; + + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning a new %s task for robot [%s]", + task.c_str(), + _context->requester_id().c_str()); + + _active = Active::make( + _desc, + _assign_id, + _context, + _state, + _update, + std::move(finished)); + } + + return _active; + } + + private: + Standby(Description desc) + : _desc(desc) + { + // Do nothing + } + Description _desc; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_traffic::Duration _time_estimate; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state; + ActivePtr _active = nullptr; + }; + + class Active : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + static std::shared_ptr make( + Description desc, + AssignIDPtr assign_id, + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished) + { + auto active = std::shared_ptr(new Active(desc)); + active->_assign_id = std::move(assign_id); + active->_context = std::move(context); + active->_state = std::move(state); + active->_update = std::move(update); + active->_finished = std::move(finished); + + active->_charging_update_subscription = active + ->_context + ->observe_charging_change() + .observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe( + [w = active->weak_from_this()](const auto&) + { + const auto self = w.lock(); + if (!self) + return; + + self->_consider_restart(); + }); + + active->_consider_restart(); + return active; + } + + rmf_task::Event::ConstStatePtr state() const final + { + return _state; + } + + rmf_traffic::Duration remaining_time_estimate() const final + { + if (_sequence) + return _sequence->remaining_time_estimate(); + + return rmf_traffic::Duration(0); + } + + Backup backup() const final + { + return Backup::make(0, nlohmann::json()); + } + + Resume interrupt(std::function task_is_interrupted) final + { + if (_sequence) + _sequence->interrupt(std::move(task_is_interrupted)); + + _current_target_wp = std::nullopt; + _current_waiting_for_charger = std::nullopt; + return Resume::make( + [w = weak_from_this()]() + { + if (const auto self = w.lock()) + { + self->_consider_restart(); + } + }); + } + + void cancel() final + { + if (_sequence) + _sequence->cancel(); + } + + void kill() final + { + if (_sequence) + _sequence->kill(); + } + + private: + + Active(Description desc) + : _desc(desc) + { + // Do nothing + } + void _consider_restart() + { + std::size_t target_wp = _context->dedicated_charging_wp(); + if (!_desc.specific_location.has_value()) + { + bool location_changed = true; + if (_current_target_wp.has_value()) + { + if (target_wp == *_current_target_wp) + { + location_changed = false; + } + } + + bool waiting_changed = true; + if (_current_waiting_for_charger.has_value()) + { + if (_context->waiting_for_charger() == *_current_waiting_for_charger) + { + waiting_changed = false; + } + } + + + if (!location_changed && !waiting_changed) + { + // No need to do anything, the charging location has not changed + // nor has the mode changed. + return; + } + } + else + { + target_wp = _desc.specific_location.value(); + if (_current_target_wp == target_wp) + return; + } + + _current_target_wp = target_wp; + _current_waiting_for_charger = _context->waiting_for_charger(); + + using UpdateFn = std::function; + using MakeStandby = std::function; + std::vector standbys; + + using GoToPlaceDesc = rmf_task_sequence::events::GoToPlace::Description; + standbys.push_back( + [ + target_wp, + assign_id = _assign_id, + context = _context + ](UpdateFn update) -> StandbyPtr + { + return events::GoToPlace::Standby::make( + assign_id, + context->make_get_state(), + context->task_parameters(), + *GoToPlaceDesc::make(target_wp), + update); + }); + + if (_desc.park) + { + standbys.push_back( + [ + assign_id = _assign_id, + context = _context + ](UpdateFn update) -> StandbyPtr + { + return events::WaitForCancel::Standby::make(context, assign_id); + }); + } + else + { + standbys.push_back( + [ + assign_id = _assign_id, + context = _context, + indefinite = _desc.indefinite + ](UpdateFn update) -> StandbyPtr + { + std::optional recharged_soc; + if (!indefinite) + { + recharged_soc = context->task_planner() + ->configuration().constraints().recharge_soc(); + } + + auto legacy = phases::WaitForCharge::make( + context, + context->task_parameters()->battery_system(), + recharged_soc); + + return events::LegacyPhaseShim::Standby::make( + std::move(legacy), context->worker(), context->clock(), assign_id, + std::move(update)); + }); + } + + _sequence = rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + standbys, _state, _update)->begin([]() {}, _finished); + } + + Description _desc; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + std::function _update; + std::function _finished; + rmf_rxcpp::subscription_guard _charging_update_subscription; + std::optional _current_target_wp; + std::optional _current_waiting_for_charger; + rmf_task_sequence::Event::ActivePtr _sequence; + }; +}; + //============================================================================== struct GoToChargerDescription : public rmf_task_sequence::events::Placeholder::Description { GoToChargerDescription() - : rmf_task_sequence::events::Placeholder::Description("Go to charger", "") + : rmf_task_sequence::events::Placeholder::Description("Go to place", "") { // Do nothing } @@ -54,7 +515,7 @@ struct GoToChargerDescription return events::GoToPlace::Standby::make( id, get_state, parameters, - *GoToPlace::make(context->dedicated_charger_wp()), + *GoToPlace::make(context->dedicated_charging_wp()), std::move(update)); } @@ -92,27 +553,37 @@ struct GoToChargerDescription struct WaitForChargeDescription : public rmf_task_sequence::events::Placeholder::Description { - WaitForChargeDescription() + WaitForChargeDescription(bool indefinite_) : rmf_task_sequence::events::Placeholder::Description( - "Wait for charging", "") + "Wait for charging", ""), + indefinite(indefinite_) { // Do nothing } + bool indefinite; + static rmf_task_sequence::Event::StandbyPtr standby( const rmf_task_sequence::Event::AssignIDPtr& id, const std::function& get_state, const rmf_task::ConstParametersPtr& parameters, - const WaitForChargeDescription&, + const WaitForChargeDescription& desc, std::function update) { const auto state = get_state(); const auto context = state.get()->value; + std::optional recharged_soc; + if (!desc.indefinite) + { + recharged_soc = context->task_planner() + ->configuration().constraints().recharge_soc(); + } + auto legacy = phases::WaitForCharge::make( context, parameters->battery_system(), - context->task_planner()->configuration().constraints().recharge_soc()); + recharged_soc); return events::LegacyPhaseShim::Standby::make( std::move(legacy), context->worker(), context->clock(), id, @@ -149,20 +620,6 @@ struct WaitForChargeDescription } }; -//============================================================================== -// TODO(MXG): Consider making the ChargeBatteryEvent description public so it -// can be incorporated into other task types -struct ChargeBatteryEventDescription - : public rmf_task_sequence::events::Placeholder::Description -{ - ChargeBatteryEventDescription() - : rmf_task_sequence::events::Placeholder::Description( - "Charge Battery", "") - { - // Do nothing - } -}; - //============================================================================== void add_charge_battery( rmf_task::Activator& task_activator, @@ -172,46 +629,128 @@ void add_charge_battery( { using Bundle = rmf_task_sequence::events::Bundle; using Phase = rmf_task_sequence::phases::SimplePhase; - using ChargeBattery = rmf_task::requests::ChargeBattery; + using ChargeBatteryTask = rmf_task::requests::ChargeBattery; - auto private_initializer = - std::make_shared(); - - WaitForChargeDescription::add(*private_initializer); - GoToChargerDescription::add(*private_initializer); - - auto charge_battery_event_unfolder = - [](const ChargeBatteryEventDescription&) + event_initializer.add( + []( + const rmf_task::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const ChargeBatteryEvent::Description& description, + std::function update) -> rmf_task_sequence::Event::StandbyPtr { - return Bundle::Description({ - std::make_shared(), - std::make_shared() - }, Bundle::Sequence, "Charge Battery"); - }; - - Bundle::unfold( - std::move(charge_battery_event_unfolder), - event_initializer, private_initializer); + return ChargeBatteryEvent::Standby::make( + id, get_state, parameters, description, std::move(update)); + }, + []( + const rmf_task::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const ChargeBatteryEvent::Description& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> rmf_task_sequence::Event::ActivePtr + { + return ChargeBatteryEvent::Standby::make( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); auto charge_battery_task_unfolder = - [](const rmf_task::requests::ChargeBattery::Description&) + [](const rmf_task::requests::ChargeBattery::Description& desc) { rmf_task_sequence::Task::Builder builder; builder .add_phase( Phase::Description::make( - std::make_shared(), + std::make_shared( + std::nullopt, desc.indefinite(), false), "Charge Battery", ""), {}); return *builder.build("Charge Battery", ""); }; - rmf_task_sequence::Task::unfold( + rmf_task_sequence::Task::unfold( std::move(charge_battery_task_unfolder), task_activator, phase_activator, std::move(clock)); } +//============================================================================== +namespace { +std::string generate_uuid(const std::size_t length = 3) +{ + std::stringstream ss; + for (std::size_t i = 0; i < length; ++i) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(0, 255); + const auto random_char = dis(gen); + std::stringstream hexstream; + hexstream << std::hex << random_char; + auto hex = hexstream.str(); + ss << (hex.length() < 2 ? '0' + hex : hex); + } + return ss.str(); +} + +} // anonymous namespace + +//============================================================================== +class ParkRobotIndefinitely::Implementation +{ +public: + std::string requester; + std::function time_now_cb; + std::optional parking_waypoint; +}; + +//============================================================================== +ParkRobotIndefinitely::ParkRobotIndefinitely( + const std::string& requester, + std::function time_now_cb, + std::optional parking_waypoint) +: _pimpl(rmf_utils::make_impl(Implementation { + requester, + time_now_cb, + parking_waypoint + })) +{ + // Do nothing +} + +//============================================================================== +rmf_task::ConstRequestPtr ParkRobotIndefinitely::make_request( + const rmf_task::State& state) const +{ + std::string id = "ParkRobot-" + generate_uuid(); + auto phase_desc = std::make_shared( + _pimpl->parking_waypoint, true, true); + + auto desc = rmf_task_sequence::Task::Builder() + .add_phase(rmf_task_sequence::phases::SimplePhase::Description::make( + phase_desc), {}) + .build("Park", ""); + + auto now = _pimpl->time_now_cb ? + _pimpl->time_now_cb() : state.time().value_or( + rmf_traffic::Time(std::chrono::system_clock::now().time_since_epoch())); + rmf_task::Task::ConstBookingPtr booking = + std::make_shared( + std::move(id), + now, + nullptr, + _pimpl->requester, + now, + true); + + return std::make_shared( + std::move(booking), + std::move(desc)); +} + } // namespace task } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp index 55657bbcf..b0215f55f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -33,14 +33,6 @@ namespace rmf_fleet_adapter { namespace tasks { -//============================================================================== -std::shared_ptr make_charge_battery( - const rmf_task::ConstRequestPtr request, - const agv::RobotContextPtr& context, - const rmf_traffic::agv::Plan::Start start, - const rmf_traffic::Time deployment_time, - const rmf_task::State finish_state); - //============================================================================== void add_charge_battery( rmf_task::Activator& task_activator, diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index 6e2a76c1f..8e2d23798 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -60,6 +60,8 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") test->received_requests.emplace_back(*lift_request); test->received_requests_cv.notify_all(); }); + auto lift_state_pub = data->ros_node->create_publisher( + LiftStateTopicName, 10); const auto info = add_robot(); const auto& context = info.context; @@ -70,41 +72,13 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") context, lift_name, destination, - context->now() + std::chrono::seconds(5), - RequestLift::Located::Outside - ); + RequestLift::Data{ + context->now() + std::chrono::seconds(5), + RequestLift::Located::Outside, + std::make_shared(0) + }); auto active_phase = pending_phase->begin(); - WHEN("it is cancelled before its started") - { - THEN("it should not send lift requests") - { - bool received_open = false; - rxcpp::composite_subscription rx_sub; - auto subscription = - data->adapter->node()->create_subscription( - AdapterLiftRequestTopicName, 10, - [&rx_sub, &received_open](LiftRequest::UniquePtr lift_request) - { - if (lift_request->request_type != LiftRequest::REQUEST_END_SESSION) - received_open = true; - else - rx_sub.unsubscribe(); - }); - - auto obs = active_phase->observe(); - active_phase->cancel(); - - // TODO(MXG): Put an explicit timeout here so this line doesn't hang - // forever in the event of a failure. - obs.as_blocking().subscribe(rx_sub); - CHECK(!received_open); - - // Stop before destructing subscription to avoid a data race in rclcpp - data->node->stop(); - } - } - WHEN("it is started") { rmf_rxcpp::subscription_guard sub = active_phase->observe().subscribe( @@ -129,8 +103,16 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") CHECK(test->received_requests.front().destination_floor == destination); } - THEN("it should continuously send lift requests") + THEN("it should continuously send lift requests while lift states arrive") { + auto t = std::thread([&]() + { + for (std::size_t i = 0; i < 10; ++i) + { + lift_state_pub->publish(LiftState()); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }); std::unique_lock lk(test->m); test->received_requests_cv.wait(lk, [test]() { @@ -140,12 +122,12 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") { CHECK(lift_request.destination_floor == destination); } + + t.join(); } AND_WHEN("lift is on destination floor") { - auto lift_state_pub = data->ros_node->create_publisher( - LiftStateTopicName, 10); auto w_lift_state_pub = std::weak_ptr>(lift_state_pub); rclcpp::TimerBase::SharedPtr timer = data->node->try_create_wall_timer( @@ -198,29 +180,6 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") timer.reset(); } - - AND_WHEN("it is cancelled") - { - { - std::unique_lock lk(test->m); - test->received_requests_cv.wait(lk, [&]() - { - return !test->received_requests.empty(); - }); - active_phase->cancel(); - } - - THEN("it should send END_SESSION request") - { - std::unique_lock lk(test->m); - test->received_requests_cv.wait(lk, [&]() - { - if (test->received_requests.empty()) - return false; - return test->received_requests.back().request_type == LiftRequest::REQUEST_END_SESSION; - }); - } - } } data->node->stop(); diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index ac83f9a68..99c028e5b 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -155,6 +155,11 @@ PYBIND11_MODULE(rmf_adapter, m) { { return self.maximum_delay(); }) + .def("current_task_id", + [&](agv::RobotUpdateHandle& self) + { + return self.current_task_id(); + }) .def("set_infinite_delay", [&](agv::RobotUpdateHandle& self) { @@ -241,6 +246,12 @@ PYBIND11_MODULE(rmf_adapter, m) { { return self.unstable().be_stubborn(); }) + .def("unstable_debug_positions", + [&](agv::RobotUpdateHandle& self, bool on) + { + self.unstable().debug_positions(on); + }, + py::arg("on")) .def("set_action_executor", &agv::RobotUpdateHandle::set_action_executor, py::arg("action_executor")) @@ -291,7 +302,8 @@ PYBIND11_MODULE(rmf_adapter, m) { return lhs == rhs; }); - py::class_( + py::class_>( m_robot_update_handle, "ActionExecution") .def("update_remaining_time", &ActionExecution::update_remaining_time, @@ -812,7 +824,12 @@ PYBIND11_MODULE(rmf_adapter, m) { &agv::EasyFullControl::RobotCallbacks::stop) .def_property_readonly( "action_executor", - &agv::EasyFullControl::RobotCallbacks::action_executor); + &agv::EasyFullControl::RobotCallbacks::action_executor) + .def_property( + "localize", + &agv::EasyFullControl::RobotCallbacks::localize, + &agv::EasyFullControl::RobotCallbacks::with_localization + ); py::class_(m_easy_full_control, "CommandExecution") .def("finished", &agv::EasyFullControl::CommandExecution::finished) @@ -820,7 +837,7 @@ PYBIND11_MODULE(rmf_adapter, m) { .def( "override_schedule", []( - ActionExecution& self, + agv::EasyFullControl::CommandExecution& self, std::string map, std::vector path, double hold) @@ -841,8 +858,10 @@ PYBIND11_MODULE(rmf_adapter, m) { .def_property_readonly("xy", &agv::EasyFullControl::Destination::xy) .def_property_readonly("yaw", &agv::EasyFullControl::Destination::yaw) .def_property_readonly("graph_index", &agv::EasyFullControl::Destination::graph_index) + .def_property_readonly("name", &agv::EasyFullControl::Destination::name) .def_property_readonly("dock", &agv::EasyFullControl::Destination::dock) - .def_property_readonly("speed_limit", &agv::EasyFullControl::Destination::speed_limit); + .def_property_readonly("speed_limit", &agv::EasyFullControl::Destination::speed_limit) + .def_property_readonly("inside_lift", &agv::EasyFullControl::Destination::inside_lift); py::class_(m_easy_full_control, "FleetConfiguration") .def(py::init([]( // Lambda function to convert reference to shared ptr @@ -1031,7 +1050,13 @@ PYBIND11_MODULE(rmf_adapter, m) { .def_property( "default_min_lane_length", &agv::EasyFullControl::FleetConfiguration::default_min_lane_length, - &agv::EasyFullControl::FleetConfiguration::set_default_min_lane_length); + &agv::EasyFullControl::FleetConfiguration::set_default_min_lane_length) + .def_property_readonly( + "lift_emergency_lanes", + &agv::EasyFullControl::FleetConfiguration::lift_emergency_levels) + .def( + "set_lift_emergency_level", + &agv::EasyFullControl::FleetConfiguration::set_lift_emergency_level); // Transformation ============================================================= py::class_(m, "Transformation") diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index a540c2408..d65f880de 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -25,6 +25,17 @@ using OrientationConstraint = Graph::OrientationConstraint; void bind_graph(py::module& m) { auto m_graph = m.def_submodule("graph"); + py::class_>(m_graph, "LiftProperties") + .def_property_readonly("name", + &Graph::LiftProperties::name) + .def_property_readonly("location", + &Graph::LiftProperties::location) + .def_property_readonly("orientation", + &Graph::LiftProperties::orientation) + .def_property_readonly("dimensions", + &Graph::LiftProperties::dimensions) + .def("is_in_lift", &Graph::LiftProperties::is_in_lift); // WAYPOINT ================================================================== py::class_(m_graph, "Waypoint") @@ -103,7 +114,7 @@ void bind_graph(py::module& m) bind_lane(m_graph); // GRAPH ===================================================================== - py::class_(m_graph, "Graph") + py::class_>(m_graph, "Graph") .def(py::init<>()) // Waypoints From 9e90fd7610bb49a1ea61e0d1bed5228d4453782a Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 15 Dec 2023 11:23:41 +0800 Subject: [PATCH 008/119] Update changelog and bump version (#311) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- rmf_charging_schedule/CHANGELOG.rst | 2 +- rmf_charging_schedule/package.xml | 2 +- rmf_fleet_adapter/CHANGELOG.rst | 4 ++++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 3 +++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 3 +++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 3 +++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 3 +++ rmf_websocket/package.xml | 2 +- 12 files changed, 23 insertions(+), 7 deletions(-) diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst index bbd8f97d9..2955530ac 100644 --- a/rmf_charging_schedule/CHANGELOG.rst +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -2,6 +2,6 @@ Changelog for package rmf_charging_schedule ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.3.2 (XXXX-YY-ZZ) +2.4.0 (2023-12-15) ------------------ * First release diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index f53610f8c..70ef0b94a 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -2,7 +2,7 @@ rmf_charging_schedule - 2.3.2 + 2.4.0 Node for a fixed 24-hour rotating charger usage schedule Grey Apache License 2.0 diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index f632aeb22..8da420f87 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.0 (2023-12-15) +------------------ +* Mutex Groups, localization hook, dynamic charging, and new graph elements (`#310 `_) + 2.3.2 (2023-08-28) ------------------ * Improve linking time (`#297 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index bd99e4cc4..32345bc22 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.3.2 + 2.4.0 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index 5b4506f5a..6c7392978 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.0 (2023-12-15) +------------------ + 2.3.2 (2023-08-28) ------------------ * EasyFullControl API (`#235 `_) diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index b737558a4..340238924 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.3.2 + 2.4.0 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index df19cbe15..03d119f50 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.0 (2023-12-15) +------------------ + 2.3.2 (2023-08-28) ------------------ * Improve linking time (`#297 `_) diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 818684d7d..3f99ff959 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.3.2 + 2.4.0 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index 8eda4437f..042b5f4e2 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.0 (2023-12-15) +------------------ + 2.3.2 (2023-08-28) ------------------ * Improve linking time (`#297 `_) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index cf5e3e29e..62dd7f9e8 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.3.2 + 2.4.0 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index 6f1d57a61..871ac8afe 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.0 (2023-12-15) +------------------ + 2.3.2 (2023-08-28) ------------------ diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index 979a0771c..7da49a00d 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.3.2 + 2.4.0 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From 93729d5f4352588c7a5b121d17c770143a0bdb46 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 19 Dec 2023 00:22:54 +0800 Subject: [PATCH 009/119] Fix merge with main Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 22 ++++++++++++------- .../rmf_fleet_adapter/events/GoToPlace.hpp | 3 ++- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 66712eb04..362962346 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -540,7 +540,7 @@ void GoToPlace::Active::_find_plan() _find_path_service) .observe_on(rxcpp::identity_same_worker(_context->worker())) .subscribe( - [w = weak_from_this(), start_name, goal_name]( + [w = weak_from_this(), start_name, goal_name, goal = *_chosen_goal]( const services::FindPath::Result& result) { const auto self = w.lock(); @@ -555,6 +555,9 @@ void GoToPlace::Active::_find_plan() "Failed to find a plan to move from [" + start_name + "] to [" + goal_name + "]. Will retry soon."); + // Reset the chosen goal in case this goal has become impossible to + // reach + self->_chosen_goal = std::nullopt; self->_execution = std::nullopt; self->_schedule_retry(); @@ -576,7 +579,8 @@ void GoToPlace::Active::_find_plan() self->_execute_plan( self->_context->itinerary().assign_plan_id(), *std::move(result), - std::move(full_itinerary)); + std::move(full_itinerary), + goal); self->_find_path_service = nullptr; self->_retry_timer = nullptr; @@ -633,7 +637,8 @@ void GoToPlace::Active::_schedule_retry() void GoToPlace::Active::_execute_plan( const rmf_traffic::PlanId plan_id, rmf_traffic::agv::Plan plan, - rmf_traffic::schedule::Itinerary full_itinerary) + rmf_traffic::schedule::Itinerary full_itinerary, + rmf_traffic::agv::Plan::Goal goal) { if (_is_interrupted) return; @@ -647,11 +652,11 @@ void GoToPlace::Active::_execute_plan( _context->node()->get_logger(), "Robot [%s] is already at its goal [%lu]", _context->requester_id().c_str(), - _goal.waypoint()); + goal.waypoint()); const auto& graph = _context->navigation_graph(); _context->retain_mutex_groups( - {graph.get_waypoint(_goal.waypoint()).in_mutex_group()}); + {graph.get_waypoint(goal.waypoint()).in_mutex_group()}); _finished(); return; @@ -667,7 +672,8 @@ void GoToPlace::Active::_execute_plan( _context->requester_id().c_str()); _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), _goal, std::move(full_itinerary), + _context, plan_id, std::move(plan), std::move(goal), + std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); if (!_execution.has_value()) @@ -701,7 +707,7 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( return nullptr; } - auto approval_cb = [w = weak_from_this()]( + auto approval_cb = [w = weak_from_this(), goal = *_chosen_goal]( const rmf_traffic::PlanId plan_id, const rmf_traffic::agv::Plan& plan, rmf_traffic::schedule::Itinerary itinerary) @@ -709,7 +715,7 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( { if (auto self = w.lock()) { - self->_execute_plan(plan_id, plan, std::move(itinerary)); + self->_execute_plan(plan_id, plan, std::move(itinerary), goal); return self->_context->itinerary().version(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index acc72f9ee..fca095903 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -116,7 +116,8 @@ class GoToPlace : public rmf_task_sequence::Event void _execute_plan( rmf_traffic::PlanId plan_id, rmf_traffic::agv::Plan plan, - rmf_traffic::schedule::Itinerary full_itinerary); + rmf_traffic::schedule::Itinerary full_itinerary, + rmf_traffic::agv::Plan::Goal goal); void _stop_and_clear(); From 433dc94c61eac99887ac24b0aeec89813a17543b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Dec 2023 12:28:27 +0800 Subject: [PATCH 010/119] Fix same floor constraint parsing Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../schemas/event_description__go_to_place.json | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/schemas/event_description__go_to_place.json b/rmf_fleet_adapter/schemas/event_description__go_to_place.json index 2d78a2070..11bff60d1 100644 --- a/rmf_fleet_adapter/schemas/event_description__go_to_place.json +++ b/rmf_fleet_adapter/schemas/event_description__go_to_place.json @@ -43,7 +43,7 @@ "properties": { "category": { "type": "string", - "description": "The type of constraint, e.g. on_same_map" + "description": "The type of constraint, e.g. prefer_same_map" }, "description": { "description": "A complete description of the constraint" diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp index c3358627e..9c7e772d1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp @@ -69,21 +69,23 @@ void add_patrol( std::make_move_iterator(place.errors.begin()), std::make_move_iterator(place.errors.end())); } - auto desc = GoToPlace::Description::make_for_one_of(goals); - return {desc, errors}; + auto desc = GoToPlace::Description::make_for_one_of(goals); const auto constraints = msg.find("constraints"); if (constraints != msg.end()) { for (const auto& constraint : constraints.value()) { - if (constraint["type"].get() == "prefer_same_map") + if (constraint["category"].get() == "prefer_same_map") { desc->prefer_same_map(true); } } } + + return {desc, errors}; } + const auto place_it = msg.find("place"); if (place_it == msg.end()) place_msg = msg; From 8c8ad1dc7ed5938ba9225bc015d791970172ba17 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 31 Oct 2023 13:07:12 +0800 Subject: [PATCH 011/119] Fix a minor typo in fleet adapter error log Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 2e148d22d..0f3b462b4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -440,7 +440,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Fleet [%s] does not have any robots to accept task [%s]. Use " - "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ", + "FleetUpdateHandle::add_robot(~) to add robots to this fleet. ", name.c_str(), task_id.c_str()); return; } From 9c062431e26440c13c31afebdc6f049b9ea50e9d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 Nov 2023 14:26:57 +0800 Subject: [PATCH 012/119] Add Reservation System node This commit adds the rust reservation system node. It will require the ros2_rust system to be set up. Signed-off-by: Arjo Chakravarty --- .gitignore | 1 + rmf_reservation_ros2/Cargo.lock | 1371 ++++++++++++++++++ rmf_reservation_ros2/Cargo.toml | 24 + rmf_reservation_ros2/package.xml | 23 + rmf_reservation_ros2/src/reservation_node.rs | 110 ++ 5 files changed, 1529 insertions(+) create mode 100644 rmf_reservation_ros2/Cargo.lock create mode 100644 rmf_reservation_ros2/Cargo.toml create mode 100644 rmf_reservation_ros2/package.xml create mode 100644 rmf_reservation_ros2/src/reservation_node.rs diff --git a/.gitignore b/.gitignore index 652199902..a3a98fa38 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ tags *.swo *.vscode compile_commands.json +rmf_reservation_ros2/target diff --git a/rmf_reservation_ros2/Cargo.lock b/rmf_reservation_ros2/Cargo.lock new file mode 100644 index 000000000..22165eadb --- /dev/null +++ b/rmf_reservation_ros2/Cargo.lock @@ -0,0 +1,1371 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "addr2line" +version = "0.21.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a30b2e23b9e17a9f90641c7ab1549cd9b44f296d3ccbf309d2863cfe398a0cb" +dependencies = [ + "gimli", +] + +[[package]] +name = "adler" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f26201604c87b1e01bd3d98f8d5d9a8fcbb815e8cedb41ffccbeb4bf593a35fe" + +[[package]] +name = "aho-corasick" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b2969dcb958b36655471fc61f7e416fa76033bdd4bfed0678d8fee1e2d07a1f0" +dependencies = [ + "memchr", +] + +[[package]] +name = "android-tzdata" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" + +[[package]] +name = "android_system_properties" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" +dependencies = [ + "libc", +] + +[[package]] +name = "anyhow" +version = "1.0.75" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4668cab20f66d8d020e1fbc0ebe47217433c1b6c8f2040faf858554e394ace6" +dependencies = [ + "backtrace", +] + +[[package]] +name = "autocfg" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" + +[[package]] +name = "backtrace" +version = "0.3.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2089b7e3f35b9dd2d0ed921ead4f6d318c27680d4a5bd167b3ee120edb105837" +dependencies = [ + "addr2line", + "cc", + "cfg-if", + "libc", + "miniz_oxide", + "object", + "rustc-demangle", +] + +[[package]] +name = "bindgen" +version = "0.66.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2b84e06fc203107bfbad243f4aba2af864eb7db3b1cf46ea0a023b0b433d2a7" +dependencies = [ + "bitflags", + "cexpr", + "clang-sys", + "lazy_static", + "lazycell", + "log", + "peeking_take_while", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "rustc-hash", + "shlex", + "syn 2.0.38", + "which", +] + +[[package]] +name = "bitflags" +version = "2.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07" + +[[package]] +name = "builtin_interfaces" +version = "1.2.1" +dependencies = [ + "rosidl_runtime_rs", +] + +[[package]] +name = "bumpalo" +version = "3.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f30e7476521f6f8af1a1c4c0b8cc94f0bee37d91763d0ca2665f299b6cd8aec" + +[[package]] +name = "cc" +version = "1.0.83" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1174fb0b6ec23863f8b971027804a42614e347eafb0a95bf0b12cdae21fc4d0" +dependencies = [ + "libc", +] + +[[package]] +name = "cexpr" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" +dependencies = [ + "nom", +] + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f2c685bad3eb3d45a01354cedb7d5faa66194d1d58ba6e267a8de788f79db38" +dependencies = [ + "android-tzdata", + "iana-time-zone", + "js-sys", + "num-traits", + "pure-rust-locales", + "rustc-serialize", + "serde", + "wasm-bindgen", + "windows-targets", +] + +[[package]] +name = "clang-sys" +version = "1.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c688fc74432808e3eb684cae8830a86be1d66a2bd58e1f248ed0960a590baf6f" +dependencies = [ + "glob", + "libc", + "libloading", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e496a50fda8aacccc86d7529e2c1e0892dbd0f898a6b5645b5561b89c3210efa" + +[[package]] +name = "deprecate-until" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ec2e2a2d0c79afd023be50309ecf010f20bf4a2761f15e264f4ac9516aff58b" +dependencies = [ + "proc-macro2", + "quote", + "semver", + "syn 2.0.38", +] + +[[package]] +name = "either" +version = "1.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a26ae43d7bcc3b814de94796a5e736d4029efb0ee900c12e2d54c993ad1a1e07" + +[[package]] +name = "equivalent" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" + +[[package]] +name = "errno" +version = "0.3.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3e13f66a2f95e32a39eaa81f6b95d42878ca0e1db0c7543723dfe12557e860" +dependencies = [ + "libc", + "windows-sys", +] + +[[package]] +name = "fixedbitset" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80" + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "futures" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da0290714b38af9b4a7b094b8a37086d1b4e61f2df9122c3cad2577669145335" +dependencies = [ + "futures-channel", + "futures-core", + "futures-executor", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff4dd66668b557604244583e3e1e1eada8c5c2e96a6d0d6653ede395b78bbacb" +dependencies = [ + "futures-core", + "futures-sink", +] + +[[package]] +name = "futures-core" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eb1d22c66e66d9d72e1758f0bd7d4fd0bee04cad842ee34587d68c07e45d088c" + +[[package]] +name = "futures-executor" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0f4fb8693db0cf099eadcca0efe2a5a22e4550f98ed16aba6c48700da29597bc" +dependencies = [ + "futures-core", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-io" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8bf34a163b5c4c52d0478a4d757da8fb65cabef42ba90515efee0f6f9fa45aaa" + +[[package]] +name = "futures-macro" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53b153fd91e4b0147f4aced87be237c98248656bb01050b96bf3ee89220a8ddb" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.38", +] + +[[package]] +name = "futures-sink" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e36d3378ee38c2a36ad710c5d30c2911d752cb941c00c72dbabfb786a7970817" + +[[package]] +name = "futures-task" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "efd193069b0ddadc69c46389b740bbccdd97203899b48d09c5f7969591d6bae2" + +[[package]] +name = "futures-util" +version = "0.3.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a19526d624e703a3179b3d322efec918b6246ea0fa51d41124525f00f1cc8104" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-macro", + "futures-sink", + "futures-task", + "memchr", + "pin-project-lite", + "pin-utils", + "slab", +] + +[[package]] +name = "getrandom" +version = "0.2.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be4136b2a15dd319360be1c07d9933517ccf0be8f16bf62a3bee4f0d618df427" +dependencies = [ + "cfg-if", + "libc", + "wasi", +] + +[[package]] +name = "gimli" +version = "0.28.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fb8d784f27acf97159b40fc4db5ecd8aa23b9ad5ef69cdd136d3bc80665f0c0" + +[[package]] +name = "glob" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" + +[[package]] +name = "hashbrown" +version = "0.14.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f93e7192158dbcda357bdec5fb5788eebf8bbac027f3f33e719d29135ae84156" + +[[package]] +name = "home" +version = "0.5.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5444c27eef6923071f7ebcc33e3444508466a76f7a2b93da00ed6e19f30c1ddb" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "iana-time-zone" +version = "0.1.58" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8326b86b6cff230b97d0d312a6c40a60726df3332e721f72a1b035f451663b20" +dependencies = [ + "android_system_properties", + "core-foundation-sys", + "iana-time-zone-haiku", + "js-sys", + "wasm-bindgen", + "windows-core", +] + +[[package]] +name = "iana-time-zone-haiku" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" +dependencies = [ + "cc", +] + +[[package]] +name = "indexmap" +version = "2.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8adf3ddd720272c6ea8bf59463c04e0f93d0bbf7c5439b691bca2987e0270897" +dependencies = [ + "equivalent", + "hashbrown", +] + +[[package]] +name = "integer-sqrt" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "276ec31bcb4a9ee45f58bec6f9ec700ae4cf4f4f8f2fa7e06cb406bd5ffdd770" +dependencies = [ + "num-traits", +] + +[[package]] +name = "iter_tools" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "531cafdc99b3b3252bb32f5620e61d56b19415efc19900b12d1b2e7483854897" +dependencies = [ + "itertools", +] + +[[package]] +name = "itertools" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" +dependencies = [ + "either", +] + +[[package]] +name = "itoa" +version = "0.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b71991ff56294aa922b450139ee08b3bfc70982c6b2c7562771375cf73542dd4" + +[[package]] +name = "js-sys" +version = "0.3.64" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c5f195fe497f702db0f318b07fdd68edb16955aed830df8363d837542f8f935a" +dependencies = [ + "wasm-bindgen", +] + +[[package]] +name = "lazy_static" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" + +[[package]] +name = "lazycell" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" + +[[package]] +name = "leb128" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "884e2677b40cc8c339eaefcb701c32ef1fd2493d71118dc0ca4b6a736c93bd67" + +[[package]] +name = "libc" +version = "0.2.149" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a08173bc88b7955d1b3145aa561539096c421ac8debde8cbc3612ec635fee29b" + +[[package]] +name = "libloading" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b67380fd3b2fbe7527a606e18729d21c6f3951633d0500574c4dc22d2d638b9f" +dependencies = [ + "cfg-if", + "winapi", +] + +[[package]] +name = "linux-raw-sys" +version = "0.4.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da2479e8c062e40bf0066ffa0bc823de0a9368974af99c9f6df941d2c231e03f" + +[[package]] +name = "log" +version = "0.4.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" + +[[package]] +name = "memchr" +version = "2.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f665ee40bc4a3c5590afb1e9677db74a508659dfd71e126420da8274909a0167" + +[[package]] +name = "minimal-lexical" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" + +[[package]] +name = "miniz_oxide" +version = "0.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7810e0be55b428ada41041c41f32c9f1a42817901b4ccf45fa3d4b6561e74c7" +dependencies = [ + "adler", +] + +[[package]] +name = "nom" +version = "7.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" +dependencies = [ + "memchr", + "minimal-lexical", +] + +[[package]] +name = "num-traits" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" +dependencies = [ + "autocfg", +] + +[[package]] +name = "object" +version = "0.32.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9cf5f9dd3933bd50a9e1f149ec995f39ae2c496d31fd772c1fd45ebc27e902b0" +dependencies = [ + "memchr", +] + +[[package]] +name = "once_cell" +version = "1.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d" + +[[package]] +name = "ordered-float" +version = "2.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68f19d67e5a2795c94e73e0bb1cc1a7edeb2e28efd39e2e1c9b7a40c1108b11c" +dependencies = [ + "num-traits", +] + +[[package]] +name = "ordered-float" +version = "3.9.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1e1c390732d15f1d48471625cd92d154e66db2c56645e29a9cd26f4699f72dc" +dependencies = [ + "num-traits", +] + +[[package]] +name = "partial_ref" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0f728bc9b1479656e40cba507034904a8c44027c0efdbbaf6a4bdc5f2d3a910c" +dependencies = [ + "partial_ref_derive", +] + +[[package]] +name = "partial_ref_derive" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "300e1d2cb5b898b5a5342e994e0d0c367dbfe69cbf717cd307045ec9fb057581" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "pathfinding" +version = "4.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b7d06c8716de428def400ae9e5cd92463cd458d98169ed835b2da5d08cf4698" +dependencies = [ + "deprecate-until", + "fixedbitset", + "indexmap", + "integer-sqrt", + "num-traits", + "rustc-hash", + "thiserror", +] + +[[package]] +name = "peeking_take_while" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" + +[[package]] +name = "pin-project-lite" +version = "0.2.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8afb450f006bf6385ca15ef45d71d2288452bc3683ce2e2cacc0d18e4be60b58" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "ppv-lite86" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" + +[[package]] +name = "prettyplease" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae005bd773ab59b4725093fd7df83fd7892f7d8eafb48dbd7de6e024e4215f9d" +dependencies = [ + "proc-macro2", + "syn 2.0.38", +] + +[[package]] +name = "proc-macro2" +version = "1.0.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "134c189feb4956b20f6f547d2cf727d4c0fe06722b20a0eec87ed445a97f92da" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "pure-rust-locales" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed02a829e62dc2715ceb8afb4f80e298148e1345749ceb369540fe0eb3368432" + +[[package]] +name = "quote" +version = "1.0.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rand" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom", +] + +[[package]] +name = "rand_pcg" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "59cad018caf63deb318e5a4586d99a24424a364f40f1e5778c29aca23f4fc73e" +dependencies = [ + "rand_core", +] + +[[package]] +name = "rand_seeder" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf2890aaef0aa82719a50e808de264f9484b74b442e1a3a0e5ee38243ac40bdb" +dependencies = [ + "rand_core", +] + +[[package]] +name = "rclrs" +version = "0.3.1" +dependencies = [ + "bindgen", + "futures", + "rosidl_runtime_rs", +] + +[[package]] +name = "regex" +version = "1.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "380b951a9c5e80ddfd6136919eef32310721aa4aacd4889a8d39124b026ab343" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f804c7828047e88b2d32e2d7fe5a105da8ee3264f01902f796c8e067dc2483f" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f" + +[[package]] +name = "rmf_chope_msgs" +version = "0.0.0" +dependencies = [ + "builtin_interfaces", + "rosidl_runtime_rs", +] + +[[package]] +name = "rmf_reservation_ros2" +version = "0.3.1" +dependencies = [ + "anyhow", + "chrono", + "rclrs", + "rmf_chope_msgs", + "rmf_reservations", + "rosidl_runtime_rs", +] + +[[package]] +name = "rmf_reservations" +version = "0.1.0" +source = "git+https://github.com/open-rmf/rmf_reservation?branch=devel#76dc7e9986a95fe7e66cbd25f918ab095f0c0ba3" +dependencies = [ + "chrono", + "fnv", + "futures", + "iter_tools", + "itertools", + "ordered-float 3.9.2", + "pathfinding", + "rand", + "rand_pcg", + "rand_seeder", + "serde", + "serde_derive", + "term-table", + "uuid", + "varisat", +] + +[[package]] +name = "rosidl_runtime_rs" +version = "0.3.1" + +[[package]] +name = "rustc-demangle" +version = "0.1.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d626bb9dae77e28219937af045c257c28bfd3f69333c512553507f5f9798cb76" + +[[package]] +name = "rustc-hash" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" + +[[package]] +name = "rustc-serialize" +version = "0.3.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dcf128d1287d2ea9d80910b5f1120d0b8eede3fbf1abe91c40d39ea7d51e6fda" + +[[package]] +name = "rustix" +version = "0.38.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b426b0506e5d50a7d8dafcf2e81471400deb602392c7dd110815afb4eaf02a3" +dependencies = [ + "bitflags", + "errno", + "libc", + "linux-raw-sys", + "windows-sys", +] + +[[package]] +name = "semver" +version = "1.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "836fa6a3e1e547f9a2c4040802ec865b5d85f4014efe00555d7090a3dcaa1090" + +[[package]] +name = "serde" +version = "1.0.190" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91d3c334ca1ee894a2c6f6ad698fe8c435b76d504b13d436f0685d648d6d96f7" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.190" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67c5609f394e5c2bd7fc51efda478004ea80ef42fee983d5c67a65e34f32c0e3" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.38", +] + +[[package]] +name = "shlex" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7cee0529a6d40f580e7a5e6c495c8fbfe21b7b52795ed4bb5e62cdf92bc6380" + +[[package]] +name = "slab" +version = "0.4.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67" +dependencies = [ + "autocfg", +] + +[[package]] +name = "smallvec" +version = "1.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "942b4a808e05215192e39f4ab80813e599068285906cc91aa64f923db842bd5a" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.38" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e96b79aaa137db8f61e26363a0c9b47d8b4ec75da28b7d1d614c2303e232408b" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "synstructure" +version = "0.12.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f36bdaa60a83aca3921b5259d5400cbf5e90fc51931376a9bd4a0eb79aa7210f" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", + "unicode-xid", +] + +[[package]] +name = "term-table" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5e59d7fb313157de2a568be8d81e4d7f9af6e50e697702e8e00190a6566d3b8" +dependencies = [ + "lazy_static", + "regex", + "unicode-width", +] + +[[package]] +name = "thiserror" +version = "1.0.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9a7210f5c9a7156bb50aa36aed4c95afb51df0df00713949448cf9e97d382d2" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "266b2e40bc00e5a6c09c3584011e08b06f123c00362c92b975ba9843aaaa14b8" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.38", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" + +[[package]] +name = "unicode-width" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e51733f11c9c4f72aa0c160008246859e340b00807569a0da0e7a1079b27ba85" + +[[package]] +name = "unicode-xid" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" + +[[package]] +name = "uuid" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88ad59a7560b41a70d191093a945f0b87bc1deeda46fb237479708a1d6b6cdfc" +dependencies = [ + "getrandom", + "rand", + "uuid-macro-internal", +] + +[[package]] +name = "uuid-macro-internal" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d8c6bba9b149ee82950daefc9623b32bb1dacbfb1890e352f6b887bd582adaf" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.38", +] + +[[package]] +name = "varisat" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ebe609851d1e9196674ac295f656bd8601200a1077343d22b345013497807caf" +dependencies = [ + "anyhow", + "itoa", + "leb128", + "log", + "ordered-float 2.10.1", + "partial_ref", + "rustc-hash", + "serde", + "thiserror", + "varisat-checker", + "varisat-dimacs", + "varisat-formula", + "varisat-internal-macros", + "varisat-internal-proof", + "vec_mut_scan", +] + +[[package]] +name = "varisat-checker" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "135c977c5913ed6e98f6b81b8e4d322211303b7d40dae773caef7ad1de6c763b" +dependencies = [ + "anyhow", + "log", + "partial_ref", + "rustc-hash", + "smallvec", + "thiserror", + "varisat-dimacs", + "varisat-formula", + "varisat-internal-proof", +] + +[[package]] +name = "varisat-dimacs" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d1dee4e21be1f04c0a939f7ae710cced47233a578de08a1b3c7d50848402636" +dependencies = [ + "anyhow", + "itoa", + "thiserror", + "varisat-formula", +] + +[[package]] +name = "varisat-formula" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "395c5543b9bfd9076d6d3af49d6c34a4b91b0b355998c0a5ec6ed7265d364520" + +[[package]] +name = "varisat-internal-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "602ece773543d066aa7848455486c6c0422a3f214da7a2b899100f3c4f12408d" +dependencies = [ + "proc-macro2", + "quote", + "regex", + "syn 1.0.109", + "synstructure", +] + +[[package]] +name = "varisat-internal-proof" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6163bb7bc9018af077b76d64f976803d141c36a27d640f1437dddc4fd527d207" +dependencies = [ + "anyhow", + "varisat-formula", +] + +[[package]] +name = "vec_mut_scan" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68ed610a8d5e63d9c0e31300e8fdb55104c5f21e422743a9dc74848fa8317fd2" + +[[package]] +name = "wasi" +version = "0.11.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" + +[[package]] +name = "wasm-bindgen" +version = "0.2.87" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7706a72ab36d8cb1f80ffbf0e071533974a60d0a308d01a5d0375bf60499a342" +dependencies = [ + "cfg-if", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.87" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5ef2b6d3c510e9625e5fe6f509ab07d66a760f0885d858736483c32ed7809abd" +dependencies = [ + "bumpalo", + "log", + "once_cell", + "proc-macro2", + "quote", + "syn 2.0.38", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.87" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dee495e55982a3bd48105a7b947fd2a9b4a8ae3010041b9e0faab3f9cd028f1d" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.87" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54681b18a46765f095758388f2d0cf16eb8d4169b639ab575a8f5693af210c7b" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.38", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.87" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ca6ad05a4870b2bf5fe995117d3728437bd27d7cd5f06f13c17443ef369775a1" + +[[package]] +name = "which" +version = "4.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" +dependencies = [ + "either", + "home", + "once_cell", + "rustix", +] + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows-core" +version = "0.51.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1f8cf84f35d2db49a46868f947758c7a1138116f7fac3bc844f43ade1292e64" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-sys" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" + +[[package]] +name = "windows_i686_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" + +[[package]] +name = "windows_i686_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" + +[[patch.unused]] +name = "action_msgs" +version = "1.2.1" + +[[patch.unused]] +name = "actionlib_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "composition_interfaces" +version = "1.2.1" + +[[patch.unused]] +name = "diagnostic_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "example_interfaces" +version = "0.9.3" + +[[patch.unused]] +name = "examples_rclrs_message_demo" +version = "0.3.1" + +[[patch.unused]] +name = "examples_rclrs_minimal_client_service" +version = "0.3.1" + +[[patch.unused]] +name = "examples_rclrs_minimal_pub_sub" +version = "0.3.1" + +[[patch.unused]] +name = "geometry_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "lifecycle_msgs" +version = "1.2.1" + +[[patch.unused]] +name = "nav_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "rcl_interfaces" +version = "1.2.1" + +[[patch.unused]] +name = "rclrs_example_msgs" +version = "0.3.1" + +[[patch.unused]] +name = "rclrs_tests" +version = "0.3.1" + +[[patch.unused]] +name = "rmf_building_map_msgs" +version = "1.4.0" + +[[patch.unused]] +name = "rmf_charger_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_dispenser_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_door_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_fleet_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_ingestor_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_lift_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_obstacle_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_reservation_ros2" +version = "0.3.1" + +[[patch.unused]] +name = "rmf_scheduler_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_site_map_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_task_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_traffic_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "rmf_visualization_msgs" +version = "1.4.0" + +[[patch.unused]] +name = "rmf_workcell_msgs" +version = "3.2.0" + +[[patch.unused]] +name = "ros_gz_interfaces" +version = "0.244.11" + +[[patch.unused]] +name = "ros_ign_interfaces" +version = "0.244.11" + +[[patch.unused]] +name = "rosgraph_msgs" +version = "1.2.1" + +[[patch.unused]] +name = "sensor_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "shape_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "statistics_msgs" +version = "1.2.1" + +[[patch.unused]] +name = "std_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "std_srvs" +version = "4.2.3" + +[[patch.unused]] +name = "stereo_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "test_msgs" +version = "1.2.1" + +[[patch.unused]] +name = "trajectory_msgs" +version = "4.2.3" + +[[patch.unused]] +name = "unique_identifier_msgs" +version = "2.2.1" + +[[patch.unused]] +name = "visualization_msgs" +version = "4.2.3" diff --git a/rmf_reservation_ros2/Cargo.toml b/rmf_reservation_ros2/Cargo.toml new file mode 100644 index 000000000..e132ff841 --- /dev/null +++ b/rmf_reservation_ros2/Cargo.toml @@ -0,0 +1,24 @@ +[package] +name = "rmf_reservation_ros2" +version = "0.3.1" +authors = ["Arjo Chakravaty "] +edition = "2021" + +[[bin]] +name = "reservation_node" +path = "src/reservation_node.rs" + +[dependencies] +anyhow = {version = "1", features = ["backtrace"]} +rmf_reservations = {git = "https://github.com/open-rmf/rmf_reservation", branch = "devel"} +# TODO(arjo): Remove chrono as un upstream dependency +chrono = "*" + +[dependencies.rclrs] +version = "0.3" + +[dependencies.rosidl_runtime_rs] +version = "0.3" + +[dependencies.rmf_chope_msgs] +version = "*" diff --git a/rmf_reservation_ros2/package.xml b/rmf_reservation_ros2/package.xml new file mode 100644 index 000000000..59c5ac748 --- /dev/null +++ b/rmf_reservation_ros2/package.xml @@ -0,0 +1,23 @@ + + + + rmf_reservation_ros2 + 0.3.1 + Package containing an example of the publish-subscribe mechanism in rclrs. + Arjo Chakravarty + Apache License 2.0 + + rclrs + rosidl_runtime_rs + std_msgs + + rclrs + rosidl_runtime_rs + rmf_chope_msgs + + + ament_cargo + + \ No newline at end of file diff --git a/rmf_reservation_ros2/src/reservation_node.rs b/rmf_reservation_ros2/src/reservation_node.rs new file mode 100644 index 000000000..ac18c71ff --- /dev/null +++ b/rmf_reservation_ros2/src/reservation_node.rs @@ -0,0 +1,110 @@ +use std::env; + +use anyhow::{Error, Result}; +use rmf_chope_msgs::{srv::{ClaimReservation_Response, ClaimReservation_Request}, msg::FixedTimeReservationAlt}; +use rmf_reservations::{database::{FixedTimeReservationSystem, Ticket}, ReservationParameters, ReservationRequest, cost_function::static_cost::StaticCost, StartTimeRange}; + +use std::sync::{Arc, Mutex}; + +struct ReservationService { + res_sys: FixedTimeReservationSystem +} + +fn reservation_req_service(service: Arc>) -> impl Fn( + &rclrs::rmw_request_id_t, + rmf_chope_msgs::srv::FixedTimeRequest_Request) -> rmf_chope_msgs::srv::FixedTimeRequest_Response { + + let service = service.clone(); + move |_req_id, requested_alternatives| { + let mut service = service.lock().unwrap(); + let mut alternatives: Vec<_> = requested_alternatives.alternatives.iter().map(|requested_alternatives| { + let start_time = chrono::DateTime::::from_timestamp(requested_alternatives.start_time.sec.into(), requested_alternatives.start_time.nanosec.into()); + + ReservationRequest { + parameters: ReservationParameters { + resource_name: requested_alternatives.resource_name.clone(), + duration: if requested_alternatives.has_end { + Some(chrono::Duration::nanoseconds(requested_alternatives.duration.nanosec.into()) + chrono::Duration::seconds(requested_alternatives.duration.sec.into())) + } else { + None + }, + start_time: StartTimeRange::exactly_at(&start_time.unwrap()) + }, + cost_function: Arc::new(StaticCost::new(requested_alternatives.cost)) + } + }).collect(); + + if alternatives.len() == 0 { + return rmf_chope_msgs::srv::FixedTimeRequest_Response { + ticket: 0, + ok: false + }; + } + + if let Ok(req) = service.res_sys.request_resources(alternatives) { + rmf_chope_msgs::srv::FixedTimeRequest_Response { + ticket: req.get_id() as u64, + ok: true + } + } + else { + rmf_chope_msgs::srv::FixedTimeRequest_Response { + ticket: 0, + ok: false + } + } + } +} + + +fn reservation_claim_service(service: Arc>) -> impl Fn( + &rclrs::rmw_request_id_t, + rmf_chope_msgs::srv::ClaimReservation_Request) -> rmf_chope_msgs::srv::ClaimReservation_Response { + + let service = service.clone(); + move |_req_id, claim| { + let mut service = service.lock().unwrap(); + + if let Some(result)= service.res_sys.claim_request(Ticket::from_id(claim.ticket as usize)) { + rmf_chope_msgs::srv::ClaimReservation_Response { + alternative: result as u64, + ok: true + } + } + else { + rmf_chope_msgs::srv::ClaimReservation_Response { + alternative: 0, + ok: false + } + } + + } +} + + +fn main() -> Result<(), Error> { + let context = rclrs::Context::new(env::args())?; + + let node = rclrs::create_node(&context, "rmf_reservation_service")?; + + let res_sys = ReservationService { + res_sys: FixedTimeReservationSystem::create_with_resources( + vec!["pantry", "tinyRobot1_charger", "lounge", "supplies"].iter().map(|x| x.to_string()).collect() + ) + }; + + let res_sys = Arc::new(Mutex::new(res_sys)); + + let reservation_req_cb = reservation_req_service(res_sys.clone()); + + let claim_cb = reservation_claim_service(res_sys.clone()); + + let _res_server = node + .create_service::("/rmf/reservation_node/request", reservation_req_cb)?; + + let _claim_server = node + .create_service::("/rmf/reservation_node/claim", claim_cb)?; + + println!("Starting server"); + rclrs::spin(node).map_err(|err| err.into()) +} From 0eba733b7e5ae333fce8dc6446ad9a9e907103c3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 10 Jan 2024 17:45:01 +0800 Subject: [PATCH 013/119] Rework node for new flexible time API Signed-off-by: Arjo Chakravarty --- rmf_reservation_ros2/Cargo.lock | 422 +++++++++---------- rmf_reservation_ros2/Cargo.toml | 9 +- rmf_reservation_ros2/package.xml | 1 + rmf_reservation_ros2/src/reservation_node.rs | 225 ++++++---- 4 files changed, 357 insertions(+), 300 deletions(-) diff --git a/rmf_reservation_ros2/Cargo.lock b/rmf_reservation_ros2/Cargo.lock index 22165eadb..ad74de243 100644 --- a/rmf_reservation_ros2/Cargo.lock +++ b/rmf_reservation_ros2/Cargo.lock @@ -43,9 +43,9 @@ dependencies = [ [[package]] name = "anyhow" -version = "1.0.75" +version = "1.0.79" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a4668cab20f66d8d020e1fbc0ebe47217433c1b6c8f2040faf858554e394ace6" +checksum = "080e9890a082662b09c1ad45f567faeeb47f22b5fb23895fbe1e651e718e25ca" dependencies = [ "backtrace", ] @@ -90,7 +90,7 @@ dependencies = [ "regex", "rustc-hash", "shlex", - "syn 2.0.38", + "syn 2.0.48", "which", ] @@ -151,14 +151,14 @@ dependencies = [ "rustc-serialize", "serde", "wasm-bindgen", - "windows-targets", + "windows-targets 0.48.5", ] [[package]] name = "clang-sys" -version = "1.6.1" +version = "1.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c688fc74432808e3eb684cae8830a86be1d66a2bd58e1f248ed0960a590baf6f" +checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" dependencies = [ "glob", "libc", @@ -167,20 +167,20 @@ dependencies = [ [[package]] name = "core-foundation-sys" -version = "0.8.4" +version = "0.8.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e496a50fda8aacccc86d7529e2c1e0892dbd0f898a6b5645b5561b89c3210efa" +checksum = "06ea2b9bc92be3c2baa9334a323ebca2d6f074ff852cd1d7b11064035cd3868f" [[package]] name = "deprecate-until" -version = "0.1.0" +version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ec2e2a2d0c79afd023be50309ecf010f20bf4a2761f15e264f4ac9516aff58b" +checksum = "7a3767f826efbbe5a5ae093920b58b43b01734202be697e1354914e862e8e704" dependencies = [ "proc-macro2", "quote", "semver", - "syn 2.0.38", + "syn 2.0.48", ] [[package]] @@ -197,12 +197,12 @@ checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" [[package]] name = "errno" -version = "0.3.5" +version = "0.3.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ac3e13f66a2f95e32a39eaa81f6b95d42878ca0e1db0c7543723dfe12557e860" +checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -219,9 +219,9 @@ checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" [[package]] name = "futures" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da0290714b38af9b4a7b094b8a37086d1b4e61f2df9122c3cad2577669145335" +checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0" dependencies = [ "futures-channel", "futures-core", @@ -234,9 +234,9 @@ dependencies = [ [[package]] name = "futures-channel" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ff4dd66668b557604244583e3e1e1eada8c5c2e96a6d0d6653ede395b78bbacb" +checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" dependencies = [ "futures-core", "futures-sink", @@ -244,15 +244,15 @@ dependencies = [ [[package]] name = "futures-core" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eb1d22c66e66d9d72e1758f0bd7d4fd0bee04cad842ee34587d68c07e45d088c" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" [[package]] name = "futures-executor" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f4fb8693db0cf099eadcca0efe2a5a22e4550f98ed16aba6c48700da29597bc" +checksum = "a576fc72ae164fca6b9db127eaa9a9dda0d61316034f33a0a0d4eda41f02b01d" dependencies = [ "futures-core", "futures-task", @@ -261,38 +261,38 @@ dependencies = [ [[package]] name = "futures-io" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8bf34a163b5c4c52d0478a4d757da8fb65cabef42ba90515efee0f6f9fa45aaa" +checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1" [[package]] name = "futures-macro" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53b153fd91e4b0147f4aced87be237c98248656bb01050b96bf3ee89220a8ddb" +checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.48", ] [[package]] name = "futures-sink" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e36d3378ee38c2a36ad710c5d30c2911d752cb941c00c72dbabfb786a7970817" +checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" [[package]] name = "futures-task" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "efd193069b0ddadc69c46389b740bbccdd97203899b48d09c5f7969591d6bae2" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" [[package]] name = "futures-util" -version = "0.3.29" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a19526d624e703a3179b3d322efec918b6246ea0fa51d41124525f00f1cc8104" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" dependencies = [ "futures-channel", "futures-core", @@ -308,9 +308,9 @@ dependencies = [ [[package]] name = "getrandom" -version = "0.2.10" +version = "0.2.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "be4136b2a15dd319360be1c07d9933517ccf0be8f16bf62a3bee4f0d618df427" +checksum = "190092ea657667030ac6a35e305e62fc4dd69fd98ac98631e5d3a2b1575a12b5" dependencies = [ "cfg-if", "libc", @@ -319,9 +319,9 @@ dependencies = [ [[package]] name = "gimli" -version = "0.28.0" +version = "0.28.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6fb8d784f27acf97159b40fc4db5ecd8aa23b9ad5ef69cdd136d3bc80665f0c0" +checksum = "4271d37baee1b8c7e4b708028c57d816cf9d2434acb33a549475f78c181f6253" [[package]] name = "glob" @@ -331,24 +331,24 @@ checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" [[package]] name = "hashbrown" -version = "0.14.2" +version = "0.14.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93e7192158dbcda357bdec5fb5788eebf8bbac027f3f33e719d29135ae84156" +checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" [[package]] name = "home" -version = "0.5.5" +version = "0.5.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5444c27eef6923071f7ebcc33e3444508466a76f7a2b93da00ed6e19f30c1ddb" +checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" dependencies = [ - "windows-sys", + "windows-sys 0.52.0", ] [[package]] name = "iana-time-zone" -version = "0.1.58" +version = "0.1.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8326b86b6cff230b97d0d312a6c40a60726df3332e721f72a1b035f451663b20" +checksum = "b6a67363e2aa4443928ce15e57ebae94fd8949958fd1223c4cfc0cd473ad7539" dependencies = [ "android_system_properties", "core-foundation-sys", @@ -369,9 +369,9 @@ dependencies = [ [[package]] name = "indexmap" -version = "2.0.2" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8adf3ddd720272c6ea8bf59463c04e0f93d0bbf7c5439b691bca2987e0270897" +checksum = "d530e1a18b1cb4c484e6e34556a0d948706958449fca0cab753d649f2bce3d1f" dependencies = [ "equivalent", "hashbrown", @@ -412,9 +412,9 @@ checksum = "b71991ff56294aa922b450139ee08b3bfc70982c6b2c7562771375cf73542dd4" [[package]] name = "js-sys" -version = "0.3.64" +version = "0.3.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c5f195fe497f702db0f318b07fdd68edb16955aed830df8363d837542f8f935a" +checksum = "cee9c64da59eae3b50095c18d3e74f8b73c0b86d2792824ff01bbce68ba229ca" dependencies = [ "wasm-bindgen", ] @@ -439,25 +439,25 @@ checksum = "884e2677b40cc8c339eaefcb701c32ef1fd2493d71118dc0ca4b6a736c93bd67" [[package]] name = "libc" -version = "0.2.149" +version = "0.2.152" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a08173bc88b7955d1b3145aa561539096c421ac8debde8cbc3612ec635fee29b" +checksum = "13e3bf6590cbc649f4d1a3eefc9d5d6eb746f5200ffb04e5e142700b8faa56e7" [[package]] name = "libloading" -version = "0.7.4" +version = "0.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b67380fd3b2fbe7527a606e18729d21c6f3951633d0500574c4dc22d2d638b9f" +checksum = "c571b676ddfc9a8c12f1f3d3085a7b163966a8fd8098a90640953ce5f6170161" dependencies = [ "cfg-if", - "winapi", + "windows-sys 0.48.0", ] [[package]] name = "linux-raw-sys" -version = "0.4.10" +version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da2479e8c062e40bf0066ffa0bc823de0a9368974af99c9f6df941d2c231e03f" +checksum = "c4cd1a83af159aa67994778be9070f0ae1bd732942279cabb14f86f986a21456" [[package]] name = "log" @@ -467,9 +467,9 @@ checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" [[package]] name = "memchr" -version = "2.6.4" +version = "2.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f665ee40bc4a3c5590afb1e9677db74a508659dfd71e126420da8274909a0167" +checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" [[package]] name = "minimal-lexical" @@ -507,18 +507,18 @@ dependencies = [ [[package]] name = "object" -version = "0.32.1" +version = "0.32.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9cf5f9dd3933bd50a9e1f149ec995f39ae2c496d31fd772c1fd45ebc27e902b0" +checksum = "a6a622008b6e321afc04970976f62ee297fdbaa6f95318ca343e3eebb9648441" dependencies = [ "memchr", ] [[package]] name = "once_cell" -version = "1.18.0" +version = "1.19.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d" +checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" [[package]] name = "ordered-float" @@ -560,9 +560,9 @@ dependencies = [ [[package]] name = "pathfinding" -version = "4.3.2" +version = "4.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b7d06c8716de428def400ae9e5cd92463cd458d98169ed835b2da5d08cf4698" +checksum = "9b425bfb8934d227558811f4b8c94d6fe64c9069c1387deed0f586bc418271b2" dependencies = [ "deprecate-until", "fixedbitset", @@ -579,6 +579,16 @@ version = "0.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" +[[package]] +name = "petgraph" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e1d3afd2628e69da2be385eb6f2fd57c8ac7977ceeff6dc166ff1657b0e386a9" +dependencies = [ + "fixedbitset", + "indexmap", +] + [[package]] name = "pin-project-lite" version = "0.2.13" @@ -599,19 +609,19 @@ checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" [[package]] name = "prettyplease" -version = "0.2.15" +version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae005bd773ab59b4725093fd7df83fd7892f7d8eafb48dbd7de6e024e4215f9d" +checksum = "a41cf62165e97c7f814d2221421dbb9afcbcdb0a88068e5ea206e19951c2cbb5" dependencies = [ "proc-macro2", - "syn 2.0.38", + "syn 2.0.48", ] [[package]] name = "proc-macro2" -version = "1.0.69" +version = "1.0.76" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "134c189feb4956b20f6f547d2cf727d4c0fe06722b20a0eec87ed445a97f92da" +checksum = "95fc56cda0b5c3325f5fbbd7ff9fda9e02bb00bb3dac51252d2f1bfa1cb8cc8c" dependencies = [ "unicode-ident", ] @@ -624,9 +634,9 @@ checksum = "ed02a829e62dc2715ceb8afb4f80e298148e1345749ceb369540fe0eb3368432" [[package]] name = "quote" -version = "1.0.33" +version = "1.0.35" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae" +checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" dependencies = [ "proc-macro2", ] @@ -681,9 +691,10 @@ dependencies = [ [[package]] name = "rclrs" -version = "0.3.1" +version = "0.4.1" dependencies = [ "bindgen", + "cfg-if", "futures", "rosidl_runtime_rs", ] @@ -730,6 +741,7 @@ name = "rmf_reservation_ros2" version = "0.3.1" dependencies = [ "anyhow", + "builtin_interfaces", "chrono", "rclrs", "rmf_chope_msgs", @@ -740,7 +752,7 @@ dependencies = [ [[package]] name = "rmf_reservations" version = "0.1.0" -source = "git+https://github.com/open-rmf/rmf_reservation?branch=devel#76dc7e9986a95fe7e66cbd25f918ab095f0c0ba3" +source = "git+https://github.com/open-rmf/rmf_reservation?rev=41f01f8#41f01f86033e46e0aa7003318d8b037241c183af" dependencies = [ "chrono", "fnv", @@ -749,6 +761,7 @@ dependencies = [ "itertools", "ordered-float 3.9.2", "pathfinding", + "petgraph", "rand", "rand_pcg", "rand_seeder", @@ -761,7 +774,10 @@ dependencies = [ [[package]] name = "rosidl_runtime_rs" -version = "0.3.1" +version = "0.4.1" +dependencies = [ + "cfg-if", +] [[package]] name = "rustc-demangle" @@ -777,47 +793,47 @@ checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" [[package]] name = "rustc-serialize" -version = "0.3.24" +version = "0.3.25" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dcf128d1287d2ea9d80910b5f1120d0b8eede3fbf1abe91c40d39ea7d51e6fda" +checksum = "fe834bc780604f4674073badbad26d7219cadfb4a2275802db12cbae17498401" [[package]] name = "rustix" -version = "0.38.21" +version = "0.38.28" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b426b0506e5d50a7d8dafcf2e81471400deb602392c7dd110815afb4eaf02a3" +checksum = "72e572a5e8ca657d7366229cdde4bd14c4eb5499a9573d4d366fe1b599daa316" dependencies = [ "bitflags", "errno", "libc", "linux-raw-sys", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] name = "semver" -version = "1.0.20" +version = "1.0.21" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "836fa6a3e1e547f9a2c4040802ec865b5d85f4014efe00555d7090a3dcaa1090" +checksum = "b97ed7a9823b74f99c7742f5336af7be5ecd3eeafcb1507d1fa93347b1d589b0" [[package]] name = "serde" -version = "1.0.190" +version = "1.0.195" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "91d3c334ca1ee894a2c6f6ad698fe8c435b76d504b13d436f0685d648d6d96f7" +checksum = "63261df402c67811e9ac6def069e4786148c4563f4b50fd4bf30aa370d626b02" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.190" +version = "1.0.195" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67c5609f394e5c2bd7fc51efda478004ea80ef42fee983d5c67a65e34f32c0e3" +checksum = "46fe8f8603d81ba86327b23a2e9cdf49e1255fb94a4c5f297f6ee0547178ea2c" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.48", ] [[package]] @@ -837,9 +853,9 @@ dependencies = [ [[package]] name = "smallvec" -version = "1.11.1" +version = "1.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "942b4a808e05215192e39f4ab80813e599068285906cc91aa64f923db842bd5a" +checksum = "4dccd0940a2dcdf68d092b8cbab7dc0ad8fa938bf95787e1b916b0e3d0e8e970" [[package]] name = "syn" @@ -854,9 +870,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.38" +version = "2.0.48" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e96b79aaa137db8f61e26363a0c9b47d8b4ec75da28b7d1d614c2303e232408b" +checksum = "0f3531638e407dfc0814761abb7c00a5b54992b849452a0646b7f65c9f770f3f" dependencies = [ "proc-macro2", "quote", @@ -888,22 +904,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.50" +version = "1.0.56" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9a7210f5c9a7156bb50aa36aed4c95afb51df0df00713949448cf9e97d382d2" +checksum = "d54378c645627613241d077a3a79db965db602882668f9136ac42af9ecb730ad" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.50" +version = "1.0.56" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "266b2e40bc00e5a6c09c3584011e08b06f123c00362c92b975ba9843aaaa14b8" +checksum = "fa0faa943b50f3db30a20aa7e265dbc66076993efed8463e8de414e5d06d3471" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.48", ] [[package]] @@ -926,9 +942,9 @@ checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" [[package]] name = "uuid" -version = "1.5.0" +version = "1.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88ad59a7560b41a70d191093a945f0b87bc1deeda46fb237479708a1d6b6cdfc" +checksum = "5e395fcf16a7a3d8127ec99782007af141946b4795001f876d54fb0d55978560" dependencies = [ "getrandom", "rand", @@ -937,13 +953,13 @@ dependencies = [ [[package]] name = "uuid-macro-internal" -version = "1.5.0" +version = "1.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d8c6bba9b149ee82950daefc9623b32bb1dacbfb1890e352f6b887bd582adaf" +checksum = "f49e7f3f3db8040a100710a11932239fd30697115e2ba4107080d8252939845e" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.48", ] [[package]] @@ -1041,9 +1057,9 @@ checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" [[package]] name = "wasm-bindgen" -version = "0.2.87" +version = "0.2.89" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7706a72ab36d8cb1f80ffbf0e071533974a60d0a308d01a5d0375bf60499a342" +checksum = "0ed0d4f68a3015cc185aff4db9506a015f4b96f95303897bfa23f846db54064e" dependencies = [ "cfg-if", "wasm-bindgen-macro", @@ -1051,24 +1067,24 @@ dependencies = [ [[package]] name = "wasm-bindgen-backend" -version = "0.2.87" +version = "0.2.89" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5ef2b6d3c510e9625e5fe6f509ab07d66a760f0885d858736483c32ed7809abd" +checksum = "1b56f625e64f3a1084ded111c4d5f477df9f8c92df113852fa5a374dbda78826" dependencies = [ "bumpalo", "log", "once_cell", "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.48", "wasm-bindgen-shared", ] [[package]] name = "wasm-bindgen-macro" -version = "0.2.87" +version = "0.2.89" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dee495e55982a3bd48105a7b947fd2a9b4a8ae3010041b9e0faab3f9cd028f1d" +checksum = "0162dbf37223cd2afce98f3d0785506dcb8d266223983e4b5b525859e6e182b2" dependencies = [ "quote", "wasm-bindgen-macro-support", @@ -1076,22 +1092,22 @@ dependencies = [ [[package]] name = "wasm-bindgen-macro-support" -version = "0.2.87" +version = "0.2.89" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54681b18a46765f095758388f2d0cf16eb8d4169b639ab575a8f5693af210c7b" +checksum = "f0eb82fcb7930ae6219a7ecfd55b217f5f0893484b7a13022ebb2b2bf20b5283" dependencies = [ "proc-macro2", "quote", - "syn 2.0.38", + "syn 2.0.48", "wasm-bindgen-backend", "wasm-bindgen-shared", ] [[package]] name = "wasm-bindgen-shared" -version = "0.2.87" +version = "0.2.89" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ca6ad05a4870b2bf5fe995117d3728437bd27d7cd5f06f13c17443ef369775a1" +checksum = "7ab9b36309365056cd639da3134bf87fa8f3d86008abf99e612384a6eecd459f" [[package]] name = "which" @@ -1106,43 +1122,30 @@ dependencies = [ ] [[package]] -name = "winapi" -version = "0.3.9" +name = "windows-core" +version = "0.52.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +checksum = "33ab640c8d7e35bf8ba19b884ba838ceb4fba93a4e8c65a9059d08afcfc683d9" dependencies = [ - "winapi-i686-pc-windows-gnu", - "winapi-x86_64-pc-windows-gnu", + "windows-targets 0.52.0", ] [[package]] -name = "winapi-i686-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" - -[[package]] -name = "winapi-x86_64-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" - -[[package]] -name = "windows-core" -version = "0.51.1" +name = "windows-sys" +version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1f8cf84f35d2db49a46868f947758c7a1138116f7fac3bc844f43ade1292e64" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" dependencies = [ - "windows-targets", + "windows-targets 0.48.5", ] [[package]] name = "windows-sys" -version = "0.48.0" +version = "0.52.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" dependencies = [ - "windows-targets", + "windows-targets 0.52.0", ] [[package]] @@ -1151,13 +1154,28 @@ version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "windows_aarch64_gnullvm 0.48.5", + "windows_aarch64_msvc 0.48.5", + "windows_i686_gnu 0.48.5", + "windows_i686_msvc 0.48.5", + "windows_x86_64_gnu 0.48.5", + "windows_x86_64_gnullvm 0.48.5", + "windows_x86_64_msvc 0.48.5", +] + +[[package]] +name = "windows-targets" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a18201040b24831fbb9e4eb208f8892e1f50a37feb53cc7ff887feb8f50e7cd" +dependencies = [ + "windows_aarch64_gnullvm 0.52.0", + "windows_aarch64_msvc 0.52.0", + "windows_i686_gnu 0.52.0", + "windows_i686_msvc 0.52.0", + "windows_x86_64_gnu 0.52.0", + "windows_x86_64_gnullvm 0.52.0", + "windows_x86_64_msvc 0.52.0", ] [[package]] @@ -1166,42 +1184,84 @@ version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7764e35d4db8a7921e09562a0304bf2f93e0a51bfccee0bd0bb0b666b015ea" + [[package]] name = "windows_aarch64_msvc" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbaa0368d4f1d2aaefc55b6fcfee13f41544ddf36801e793edbbfd7d7df075ef" + [[package]] name = "windows_i686_gnu" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" +[[package]] +name = "windows_i686_gnu" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a28637cb1fa3560a16915793afb20081aba2c92ee8af57b4d5f28e4b3e7df313" + [[package]] name = "windows_i686_msvc" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" +[[package]] +name = "windows_i686_msvc" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ffe5e8e31046ce6230cc7215707b816e339ff4d4d67c65dffa206fd0f7aa7b9a" + [[package]] name = "windows_x86_64_gnu" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6fa32db2bc4a2f5abeacf2b69f7992cd09dca97498da74a151a3132c26befd" + [[package]] name = "windows_x86_64_gnullvm" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a657e1e9d3f514745a572a6846d3c7aa7dbe1658c056ed9c3344c4109a6949e" + [[package]] name = "windows_x86_64_msvc" version = "0.48.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dff9641d1cd4be8d1a070daf9e3773c5f67e78b4d9d42263020c057706765c04" + [[patch.unused]] name = "action_msgs" version = "1.2.1" @@ -1224,15 +1284,15 @@ version = "0.9.3" [[patch.unused]] name = "examples_rclrs_message_demo" -version = "0.3.1" +version = "0.4.1" [[patch.unused]] name = "examples_rclrs_minimal_client_service" -version = "0.3.1" +version = "0.4.1" [[patch.unused]] name = "examples_rclrs_minimal_pub_sub" -version = "0.3.1" +version = "0.4.1" [[patch.unused]] name = "geometry_msgs" @@ -1252,79 +1312,11 @@ version = "1.2.1" [[patch.unused]] name = "rclrs_example_msgs" -version = "0.3.1" +version = "0.4.1" [[patch.unused]] name = "rclrs_tests" -version = "0.3.1" - -[[patch.unused]] -name = "rmf_building_map_msgs" -version = "1.4.0" - -[[patch.unused]] -name = "rmf_charger_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_dispenser_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_door_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_fleet_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_ingestor_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_lift_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_obstacle_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_reservation_ros2" -version = "0.3.1" - -[[patch.unused]] -name = "rmf_scheduler_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_site_map_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_task_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_traffic_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "rmf_visualization_msgs" -version = "1.4.0" - -[[patch.unused]] -name = "rmf_workcell_msgs" -version = "3.2.0" - -[[patch.unused]] -name = "ros_gz_interfaces" -version = "0.244.11" - -[[patch.unused]] -name = "ros_ign_interfaces" -version = "0.244.11" +version = "0.4.1" [[patch.unused]] name = "rosgraph_msgs" diff --git a/rmf_reservation_ros2/Cargo.toml b/rmf_reservation_ros2/Cargo.toml index e132ff841..191ecd784 100644 --- a/rmf_reservation_ros2/Cargo.toml +++ b/rmf_reservation_ros2/Cargo.toml @@ -10,15 +10,18 @@ path = "src/reservation_node.rs" [dependencies] anyhow = {version = "1", features = ["backtrace"]} -rmf_reservations = {git = "https://github.com/open-rmf/rmf_reservation", branch = "devel"} +rmf_reservations = {git = "https://github.com/open-rmf/rmf_reservation", rev = "41f01f8"} # TODO(arjo): Remove chrono as un upstream dependency chrono = "*" [dependencies.rclrs] -version = "0.3" +version = "0.4.1" [dependencies.rosidl_runtime_rs] -version = "0.3" +version = "0.4.1" [dependencies.rmf_chope_msgs] version = "*" + +[dependencies.builtin_interfaces] +version = "*" diff --git a/rmf_reservation_ros2/package.xml b/rmf_reservation_ros2/package.xml index 59c5ac748..3fd2fee8b 100644 --- a/rmf_reservation_ros2/package.xml +++ b/rmf_reservation_ros2/package.xml @@ -16,6 +16,7 @@ rclrs rosidl_runtime_rs rmf_chope_msgs + builtin_interfaces ament_cargo diff --git a/rmf_reservation_ros2/src/reservation_node.rs b/rmf_reservation_ros2/src/reservation_node.rs index ac18c71ff..35b33ab08 100644 --- a/rmf_reservation_ros2/src/reservation_node.rs +++ b/rmf_reservation_ros2/src/reservation_node.rs @@ -1,110 +1,171 @@ use std::env; +use chrono::Utc; use anyhow::{Error, Result}; -use rmf_chope_msgs::{srv::{ClaimReservation_Response, ClaimReservation_Request}, msg::FixedTimeReservationAlt}; -use rmf_reservations::{database::{FixedTimeReservationSystem, Ticket}, ReservationParameters, ReservationRequest, cost_function::static_cost::StaticCost, StartTimeRange}; +use rmf_chope_msgs::msg::{FlexibleTimeRequest, DelayRequest, Ticket as RmfTicket, ClaimRequest, ReservationAllocation}; +use rmf_reservations::{database::{FlexibleTimeReservationSystem, Ticket, ClockSource, }, ReservationParameters, ReservationRequest, cost_function::static_cost::StaticCost, StartTimeRange, }; +use rmf_reservations::database::ClaimSpot; use std::sync::{Arc, Mutex}; +use std::collections::HashMap; -struct ReservationService { - res_sys: FixedTimeReservationSystem +struct ReservationSystem { + system: FlexibleTimeReservationSystem, } -fn reservation_req_service(service: Arc>) -> impl Fn( - &rclrs::rmw_request_id_t, - rmf_chope_msgs::srv::FixedTimeRequest_Request) -> rmf_chope_msgs::srv::FixedTimeRequest_Response { - - let service = service.clone(); - move |_req_id, requested_alternatives| { - let mut service = service.lock().unwrap(); - let mut alternatives: Vec<_> = requested_alternatives.alternatives.iter().map(|requested_alternatives| { - let start_time = chrono::DateTime::::from_timestamp(requested_alternatives.start_time.sec.into(), requested_alternatives.start_time.nanosec.into()); - - ReservationRequest { - parameters: ReservationParameters { - resource_name: requested_alternatives.resource_name.clone(), - duration: if requested_alternatives.has_end { - Some(chrono::Duration::nanoseconds(requested_alternatives.duration.nanosec.into()) + chrono::Duration::seconds(requested_alternatives.duration.sec.into())) - } else { - None - }, - start_time: StartTimeRange::exactly_at(&start_time.unwrap()) - }, - cost_function: Arc::new(StaticCost::new(requested_alternatives.cost)) - } - }).collect(); - - if alternatives.len() == 0 { - return rmf_chope_msgs::srv::FixedTimeRequest_Response { - ticket: 0, - ok: false - }; - } - - if let Ok(req) = service.res_sys.request_resources(alternatives) { - rmf_chope_msgs::srv::FixedTimeRequest_Response { - ticket: req.get_id() as u64, - ok: true - } - } - else { - rmf_chope_msgs::srv::FixedTimeRequest_Response { - ticket: 0, - ok: false - } +impl ReservationSystem { + pub fn create_with_clock(clock_source: RosClockSourceProvider) -> Self { + Self { + system: FlexibleTimeReservationSystem::create_with_clock(clock_source) } } } +struct RosClockSourceProvider { + node: Arc +} -fn reservation_claim_service(service: Arc>) -> impl Fn( - &rclrs::rmw_request_id_t, - rmf_chope_msgs::srv::ClaimReservation_Request) -> rmf_chope_msgs::srv::ClaimReservation_Response { - - let service = service.clone(); - move |_req_id, claim| { - let mut service = service.lock().unwrap(); - - if let Some(result)= service.res_sys.claim_request(Ticket::from_id(claim.ticket as usize)) { - rmf_chope_msgs::srv::ClaimReservation_Response { - alternative: result as u64, - ok: true - } - } - else { - rmf_chope_msgs::srv::ClaimReservation_Response { - alternative: 0, - ok: false - } - } +impl ClockSource for RosClockSourceProvider { + fn now(&self) -> chrono::DateTime { + return from_ros_time(&to_ros_msg(&self.node.get_clock().now())); + } +} + +fn to_ros_msg(time: &rclrs::Time) -> builtin_interfaces::msg::Time { + let nanosec = time.nsec % 1_000_000_000; + let sec = time.nsec / 1_000_000_000; + + builtin_interfaces::msg::Time { + nanosec: nanosec.try_into().unwrap(), + sec: sec.try_into().unwrap() + } +} +fn to_ros_time(time: &chrono::DateTime) -> builtin_interfaces::msg::Time { + builtin_interfaces::msg::Time { + nanosec: (time.timestamp_nanos() - time.timestamp() * 1_000_000_000).try_into().unwrap(), + sec: time.timestamp().try_into().unwrap() } } +fn from_ros_time(time: &builtin_interfaces::msg::Time) -> chrono::DateTime { + chrono::DateTime::::from_timestamp(time.sec.into(), time.nanosec.into()).unwrap() +} + +fn from_ros_duration(duration: &builtin_interfaces::msg::Duration) -> chrono::Duration { + chrono::Duration::nanoseconds(duration.nanosec.into()) + chrono::Duration::seconds(duration.sec.into()) +} fn main() -> Result<(), Error> { let context = rclrs::Context::new(env::args())?; - let node = rclrs::create_node(&context, "rmf_reservation_service")?; + let node = rclrs::create_node(&context, "rmf_reservation_service")?; - let res_sys = ReservationService { - res_sys: FixedTimeReservationSystem::create_with_resources( - vec!["pantry", "tinyRobot1_charger", "lounge", "supplies"].iter().map(|x| x.to_string()).collect() - ) + let clock_source= RosClockSourceProvider { + node: node.clone() }; + let reservation_system = Arc::new(Mutex::new(ReservationSystem::create_with_clock(clock_source))); + + let res_sys_handle = reservation_system.clone(); + let publisher = + node.create_publisher::("rmf/reservations/tickets", rclrs::QOS_PROFILE_DEFAULT)?; + + let _request_sub = node.create_subscription::( + "rmf/reservations/request", + rclrs::QOS_PROFILE_DEFAULT, + move |msg: FlexibleTimeRequest| { + let alternative = msg.alternatives.iter().map(|alt| { + ReservationRequest { + parameters: ReservationParameters { + resource_name: alt.resource_name.clone(), + duration: if alt.has_end { + Some(from_ros_duration(&alt.duration)) + } else { + None + }, + start_time: StartTimeRange { + earliest_start: if alt.start_time.has_earliest_start_time { + Some(from_ros_time(&alt.start_time.earliest_start_time)) + } else { + None + }, + latest_start: if alt.start_time.has_latest_start_time { + Some(from_ros_time(&alt.start_time.latest_start_time)) + } else { + None + } + }, + }, + cost_function: Arc::new(StaticCost::new(alt.cost)) + } + }); + + //println!("I heard: '{}'", msg.data); + let mut res_sys = res_sys_handle.lock().unwrap(); + let Ok(ticket) = res_sys.system.request_resources(alternative.collect()) else { + println!("Could not claim ticket. It was likely already claimed"); + return; + }; + //res_sys.mapping.insert(req_header, ticket.clone()); + let ticket = RmfTicket { + header: msg.header, + ticket_id: ticket.get_id() as u64 + }; + publisher.publish(ticket); + }, + )?; + + let res_sys_handle = reservation_system.clone(); + let publisher = + node.create_publisher::("rmf/reservations/allocation", rclrs::QOS_PROFILE_DEFAULT)?; + let _claim_sub = node.create_subscription::( + "rmf/reservations/request", + rclrs::QOS_PROFILE_DEFAULT, + move |msg: ClaimRequest| { + let mut res_sys = res_sys_handle.lock().unwrap(); + let resp = res_sys.system.claim_request(Ticket::from_id(msg.ticket.ticket_id as usize), &msg.wait_points); + let Ok(result) = resp else { + println!("Got error message while trying to claim: {}", resp.unwrap_err()); + return; + }; - let res_sys = Arc::new(Mutex::new(res_sys)); - - let reservation_req_cb = reservation_req_service(res_sys.clone()); - - let claim_cb = reservation_claim_service(res_sys.clone()); - - let _res_server = node - .create_service::("/rmf/reservation_node/request", reservation_req_cb)?; + let allocation = match result { + ClaimSpot::GoImmediately(goal) => { + ReservationAllocation { + ticket: msg.ticket.clone(), + instruction_type: ReservationAllocation::IMMEDIATELY_PROCEED, + satisfies_alternative: goal.satisfies_alt as u64, + resource: goal.resource.clone(), + time_to_reach: to_ros_time(&goal.time), + alternate_waitpoint: Default::default() + } + }, + ClaimSpot::WaitAtThenGo(id, goal) => { + ReservationAllocation { + ticket: msg.ticket.clone(), + instruction_type: ReservationAllocation::WAIT_AT_SPOT_AND_THEN_GO, + satisfies_alternative: goal.satisfies_alt as u64, + resource: goal.resource.clone(), + time_to_reach: to_ros_time(&goal.time), + alternate_waitpoint: msg.wait_points[id].clone() + } + }, + ClaimSpot::WaitPermanently(id) => { + ReservationAllocation { + ticket: msg.ticket.clone(), + instruction_type: ReservationAllocation::WAIT_PERMANENTLY, + satisfies_alternative: Default::default(), + resource: Default::default(), + time_to_reach: Default::default(), + alternate_waitpoint: msg.wait_points[id].clone() + } + } + }; - let _claim_server = node - .create_service::("/rmf/reservation_node/claim", claim_cb)?; + publisher.publish(allocation); + } + ); + - println!("Starting server"); rclrs::spin(node).map_err(|err| err.into()) } From 5e0cbe23b2ec1f67e08e8568b35eaa7d4b1b68b5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 16 Jan 2024 17:33:05 +0800 Subject: [PATCH 014/119] Adding support for reservation system in classic RMF Going to drop Rust dependency. Currently working mutex into GoToPlace. Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/StandardNames.hpp | 5 + rmf_fleet_adapter/package.xml | 1 + rmf_fleet_adapter/schemas/place.json | 3 +- .../agv/FleetUpdateHandle.cpp | 4 + .../src/rmf_fleet_adapter/agv/Node.cpp | 39 ++++++++ .../src/rmf_fleet_adapter/agv/Node.hpp | 27 ++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 9 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 + .../rmf_fleet_adapter/events/GoToPlace.cpp | 95 ++++++++++++++++++- .../rmf_fleet_adapter/events/GoToPlace.hpp | 9 ++ 10 files changed, 193 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index a736ce9e5..63cd78b0a 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -73,6 +73,11 @@ const std::string ChargingAssignmentsTopicName = "charging_assignments"; const std::string MutexGroupRequestTopicName = "mutex_group_request"; const std::string MutexGroupStatesTopicName = "mutex_group_states"; +const std::string ReservationRequestTopicName = "/rmf/reservations/request"; +const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; +const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; +const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; + const uint64_t Unclaimed = (uint64_t)(-1); } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 32345bc22..82a680b6e 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -30,6 +30,7 @@ std_msgs rmf_api_msgs rmf_building_map_msgs + rmf_chope_msgs stubborn_buddies stubborn_buddies_msgs diff --git a/rmf_fleet_adapter/schemas/place.json b/rmf_fleet_adapter/schemas/place.json index 684a35fe6..84738967a 100644 --- a/rmf_fleet_adapter/schemas/place.json +++ b/rmf_fleet_adapter/schemas/place.json @@ -11,7 +11,8 @@ "type": "object", "properties": { "waypoint": { "$ref": "#/$defs/waypoint" }, - "orientation": { "type": "number" } + "orientation": { "type": "number" }, + "hold_for": {"type": "number"} }, "required": ["waypoint"] } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 0f3b462b4..898b381af 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1397,6 +1397,10 @@ PlaceDeserializer make_place_deserializer( const auto& ori_it = msg.find("orientation"); if (ori_it != msg.end()) place->orientation(ori_it->get()); + + const auto& hold_it = msg.find("hold_for"); + if (hold_it != msg.end()) + place->holding_time(hold_it->get()); } return {place, {}}; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 322281326..21c4b8d8d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -120,6 +120,21 @@ std::shared_ptr Node::make( node->create_observable( MutexGroupStatesTopicName, transient_local_qos); + node->_reservation_request_pub = + node->create_publisher( + ReservationRequestTopicName, transient_local_qos); + + node->_reservation_ticket_obs = + node->create_observable( + ReservationResponseTopicName, transient_local_qos); + + node->_reservation_claim_pub = + node->create_publisher( + ReservationClaimTopicName, transient_local_qos); + + node->_reservation_alloc_obs = + node->create_observable( + ReservationAllocationTopicName, transient_local_qos); return node; } @@ -261,5 +276,29 @@ auto Node::mutex_group_states() const -> const MutexGroupStatesObs& return _mutex_group_states_obs->observe(); } +//============================================================================== +auto Node::location_requester() const -> const ReservationRequestPub& +{ + return _reservation_request_pub; +} + +//============================================================================== +auto Node::claim_location_ticket() const -> const ReservationClaimPub& +{ + return _reservation_claim_pub; +} + +//============================================================================== +auto Node::location_ticket_obs() const -> const ReservationTicketObs& +{ + return _reservation_ticket_obs->observe(); +} + +//============================================================================== +auto Node::allocated_claims_obs() const -> const ReservationAllocationObs& +{ + return _reservation_alloc_obs->observe(); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 5331b43e2..c3cc0b087 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -18,6 +18,8 @@ #ifndef SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP #define SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP +#include +#include #include #include @@ -32,6 +34,11 @@ #include #include #include +#include +#include +#include +#include + #include #include @@ -135,6 +142,22 @@ class Node : public rmf_rxcpp::Transport using MutexGroupStatesObs = rxcpp::observable; const MutexGroupStatesObs& mutex_group_states() const; + using ReservationRequest = rmf_chope_msgs::msg::FlexibleTimeRequest; + using ReservationRequestPub = rclcpp::Publisher::SharedPtr; + const ReservationRequestPub& location_requester() const; + + using ReservationTicket = rmf_chope_msgs::msg::Ticket; + using ReservationTicketObs = rxcpp::observable; + const ReservationTicketObs& location_ticket_obs() const; + + using ReservationClaim = rmf_chope_msgs::msg::ClaimRequest; + using ReservationClaimPub = rclcpp::Publisher::SharedPtr; + const ReservationClaimPub& claim_location_ticket() const; + + using ReservationAllocation = rmf_chope_msgs::msg::ReservationAllocation; + using ReservationAllocationObs = rxcpp::observable; + const ReservationAllocationObs& allocated_claims_obs() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( std::chrono::duration period, @@ -195,6 +218,10 @@ class Node : public rmf_rxcpp::Transport MutexGroupRequestPub _mutex_group_request_pub; Bridge _mutex_group_request_obs; Bridge _mutex_group_states_obs; + ReservationRequestPub _reservation_request_pub; + Bridge _reservation_ticket_obs; + ReservationClaimPub _reservation_claim_pub; + Bridge _reservation_alloc_obs; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 0abfd7165..17622871d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -15,6 +15,7 @@ * */ +#include "RobotContext.hpp" #include "internal_RobotUpdateHandle.hpp" #include @@ -889,6 +890,11 @@ rmf_traffic::Duration RobotContext::get_lift_rewait_duration() const return _lift_rewait_duration; } +//============================================================================== +uint64_t RobotContext::last_reservation_request_id() { + return _last_reservation_request_id++; +} + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, @@ -1127,7 +1133,8 @@ RobotContext::RobotContext( _current_task_end_state(state), _current_task_id(std::nullopt), _task_planner(std::move(task_planner)), - _reporting(_worker) + _reporting(_worker), + _last_reservation_request_id(0) { _most_recent_valid_location = _location; _profile = std::make_shared( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 9c82be580..e36382241 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -397,6 +397,9 @@ class RobotContext { public: + + uint64_t last_reservation_request_id(); + /// Get a handle to the command interface of the robot. This may return a /// nullptr if the robot has disconnected and/or its command API is no longer /// available. @@ -871,6 +874,7 @@ class RobotContext rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; std::chrono::steady_clock::time_point _last_active_task_time; + uint64_t _last_reservation_request_id; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 362962346..02a08618b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -17,7 +17,10 @@ #include "GoToPlace.hpp" #include "../project_itinerary.hpp" +#include "PerformAction.hpp" +#include +#include #include #include @@ -163,6 +166,88 @@ auto GoToPlace::Active::make( return active; } + active->_reservation_id = active->_context->last_reservation_request_id(); + active->_reservation_ticket = active->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe([w = active->weak_from_this()](const std::shared_ptr& msg) + { + const auto self = w.lock(); + if (!self) + return; + + if (msg->header.request_id != self->_reservation_id + || msg->header.robot_name != self->_context->name() + || msg->header.fleet_name != self->_context->group()) + { + return; + } + + self->_ticket = msg; + + + // Pick the nearest location to wait + auto current_location = _context->location(); + if (current_location.size() == 0) + { + return; + } + + // Order wait points by the number of + std::map waitpoints_order; + for (std::size_t i = 0; i < self->_context->navigation_graph()->num_waypoints(); i++) { + const auto wp = self->_context->navigation_graph()->get_waypoint(i); + + // Wait at parking spot and check its on same floor. + if (!wp.is_parking_spot() || wp.get_map_name() != self->_context->map) { + continue; + } + + + auto result = self->_context->planner()->quickest_path(current_location, wp_idx); + if (!result.has_value()) { + continue; + } + + + stringstream json_stream; + json_stream << i << std::endl; + string json; + json_stream >> json; + waitpoints_order.insert(result, json); + } + + std::vector waitpoints; + for (auto &[_, name]: waitpoints_order) { + waitpoints.push_back(name); + } + + // Immediately make claim cause we don't yet support flexible reservations. + rmf_chope_msgs::msg::ClaimRequest claim_request; + claim_request->ticket = *msg; + claim_request->wait_points = waitpoints; + self->_context->node()->claim_location_ticket()->publish(claim_request); + }); + + active->_reservation_allocation = active->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe([w = active->weak_from_this()](const std::shared_ptr& msg) + { + const auto self = w.lock(); + if (!self) + return; + + if (!self->_ticket.has_value()) + { + return; + } + + if (msg->ticket.ticket_id != self->_ticket.value()->ticket_id) + { + return; + } + + self->_final_allocated_destination = msg; + }); + + active->_negotiator = Negotiator::make( active->_context, @@ -422,6 +507,12 @@ std::optional GoToPlace::Active::_choose_goal( _description.one_of().size(), _context->requester_id().c_str()); + // Select node + rmf_chope_msgs::msg::FlexibleTimeRequest ftr; + ftr.header.robot_name = _context->name(); + ftr.header.fleet_name = _context->group(); + ftr.header.request_id = _reservation_id; + auto lowest_cost = std::numeric_limits::infinity(); std::optional selected_idx; for (std::size_t i = 0; i < _description.one_of().size(); ++i) @@ -469,7 +560,9 @@ std::optional GoToPlace::Active::_choose_goal( wp_idx, _context->requester_id().c_str()); } - } + } + _context->node()->location_requester()->publish(ftr); + if (selected_idx.has_value()) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index fca095903..2c5081cab 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -25,6 +25,8 @@ #include "ExecutePlan.hpp" +#include +#include #include #include #include @@ -143,6 +145,13 @@ class GoToPlace : public rmf_task_sequence::Event rmf_rxcpp::subscription_guard _replan_request_subscription; rmf_rxcpp::subscription_guard _graph_change_subscription; + rmf_rxcpp::subscription_guard _reservation_ticket; + rmf_rxcpp::subscription_guard _reservation_allocation; + + uint64_t _reservation_id = 0; + std::optional> _ticket{std::nullopt}; + std::optional> _final_allocated_destination{std::nullopt}; + bool _is_interrupted = false; }; }; From 6fe0d4f0166859c3c35db0df9f10305ace825eda Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 16 Jan 2024 17:35:31 +0800 Subject: [PATCH 015/119] Some things missing from previous push Signed-off-by: Arjo Chakravarty --- rmf_reservation_ros2/Cargo.lock | 1 + rmf_reservation_ros2/src/reservation_node.rs | 7 ++++--- rmf_traffic_ros2/CMakeLists.txt | 4 ++++ rmf_traffic_ros2/package.xml | 1 + 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/rmf_reservation_ros2/Cargo.lock b/rmf_reservation_ros2/Cargo.lock index ad74de243..944dac430 100644 --- a/rmf_reservation_ros2/Cargo.lock +++ b/rmf_reservation_ros2/Cargo.lock @@ -694,6 +694,7 @@ name = "rclrs" version = "0.4.1" dependencies = [ "bindgen", + "builtin_interfaces", "cfg-if", "futures", "rosidl_runtime_rs", diff --git a/rmf_reservation_ros2/src/reservation_node.rs b/rmf_reservation_ros2/src/reservation_node.rs index 35b33ab08..cb1d1a03d 100644 --- a/rmf_reservation_ros2/src/reservation_node.rs +++ b/rmf_reservation_ros2/src/reservation_node.rs @@ -100,6 +100,8 @@ fn main() -> Result<(), Error> { } }); + println!("Recieved request."); + //println!("I heard: '{}'", msg.data); let mut res_sys = res_sys_handle.lock().unwrap(); let Ok(ticket) = res_sys.system.request_resources(alternative.collect()) else { @@ -119,7 +121,7 @@ fn main() -> Result<(), Error> { let publisher = node.create_publisher::("rmf/reservations/allocation", rclrs::QOS_PROFILE_DEFAULT)?; let _claim_sub = node.create_subscription::( - "rmf/reservations/request", + "rmf/reservations/claim", rclrs::QOS_PROFILE_DEFAULT, move |msg: ClaimRequest| { let mut res_sys = res_sys_handle.lock().unwrap(); @@ -164,8 +166,7 @@ fn main() -> Result<(), Error> { publisher.publish(allocation); } - ); - + ); rclrs::spin(node).map_err(|err| err.into()) } diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 28692d7a0..341c098c1 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -27,6 +27,8 @@ find_package(yaml-cpp REQUIRED) find_package(nlohmann_json REQUIRED) find_package(ZLIB REQUIRED) find_package(LibUUID REQUIRED) +find_package(rmf_chope_msgs REQUIRED) + # NOTE(MXG): libproj-dev does not currently distribute its cmake config-files # in Debian, so we can't use find_package and need to rely on pkg-config. @@ -176,6 +178,7 @@ target_link_libraries(rmf_traffic_ros2 ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} ${rmf_building_map_msgs_LIBRARIES} + ${rmf_chope_msgs_LIBRARIES} ${rclcpp_LIBRARIES} yaml-cpp ZLIB::ZLIB @@ -191,6 +194,7 @@ target_include_directories(rmf_traffic_ros2 ${rmf_traffic_msgs_INCLUDE_DIRS} ${rmf_site_map_msgs_INCLUDE_DIRS} ${rmf_building_map_msgs_INCLUDE_DIRS} + ${rmf_chope_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 62dd7f9e8..812c3988b 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -15,6 +15,7 @@ rmf_traffic rmf_traffic_msgs rmf_fleet_msgs + rmf_chope_msgs rmf_site_map_msgs rmf_building_map_msgs rclcpp From 59fc9958bb28c22334fef1c3d415205d6f953750 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 26 Jan 2024 17:25:37 +0800 Subject: [PATCH 016/119] Add support for releasing reservations Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/StandardNames.hpp | 1 + .../src/rmf_fleet_adapter/agv/Node.cpp | 10 ++ .../src/rmf_fleet_adapter/agv/Node.hpp | 6 + .../agv/ReservationManager.hpp | 53 +++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 14 ++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 9 +- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 21 +++ .../rmf_fleet_adapter/events/GoToPlace.cpp | 68 ++++++--- .../phases/ReleaseResource.cpp | 133 ++++++++++++++++++ .../phases/ReleaseResource.hpp | 104 ++++++++++++++ 10 files changed, 399 insertions(+), 20 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 63cd78b0a..6cd190f60 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -77,6 +77,7 @@ const std::string ReservationRequestTopicName = "/rmf/reservations/request"; const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; +const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; const uint64_t Unclaimed = (uint64_t)(-1); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 21c4b8d8d..d72acf912 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -135,6 +135,10 @@ std::shared_ptr Node::make( node->_reservation_alloc_obs = node->create_observable( ReservationAllocationTopicName, transient_local_qos); + + node->_reservation_release_pub = + node->create_publisher( + ReservationReleaseTopicName, transient_local_qos); return node; } @@ -300,5 +304,11 @@ auto Node::allocated_claims_obs() const -> const ReservationAllocationObs& return _reservation_alloc_obs->observe(); } +//============================================================================== +auto Node::release_location() const -> const ReservationReleasePub& +{ + return _reservation_release_pub; +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index c3cc0b087..88fca8033 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include @@ -158,6 +159,10 @@ class Node : public rmf_rxcpp::Transport using ReservationAllocationObs = rxcpp::observable; const ReservationAllocationObs& allocated_claims_obs() const; + using ReservationRelease = rmf_chope_msgs::msg::ReleaseRequest; + using ReservationReleasePub = rclcpp::Publisher::SharedPtr; + const ReservationReleasePub& release_location() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( std::chrono::duration period, @@ -222,6 +227,7 @@ class Node : public rmf_rxcpp::Transport Bridge _reservation_ticket_obs; ReservationClaimPub _reservation_claim_pub; Bridge _reservation_alloc_obs; + ReservationReleasePub _reservation_release_pub; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp new file mode 100644 index 000000000..2f4a369ac --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2020 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 SRC__RMF_FLEET_ADAPTER__AGV__RESERVATION_MANAGER_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__RESERVATION_MANAGER_HPP +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace agv { + +enum class ReservationState { + AWAITING_REQUEST, + AWAITING_ALLOCATION, + AWAITING_TURN +}; + + +class ReservationManager { +public: + void add_ticket(const rmf_chope_msgs::msg::ReservationAllocation alloc) { + allocations.push_front(alloc); + } + + std::optional release_ticket() { + if (allocations.size() == 0) { + return std::nullopt; + } + auto temp = allocations.back(); + allocations.pop_back(); + return temp; + } + + std::deque allocations; +}; +} +} +#endif \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 17622871d..bc27f51ea 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1600,5 +1600,19 @@ void RobotContext::_publish_mutex_group_requests() } } +//============================================================================== +void RobotContext::_set_allocated_destination( + const rmf_chope_msgs::msg::ReservationAllocation& ticket) +{ + _reservation_mgr.add_ticket(ticket); +} + +//============================================================================== +std::optional + RobotContext::_release_resource() +{ + return _reservation_mgr.release_ticket(); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index e36382241..335386fd0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -43,7 +43,7 @@ #include "Node.hpp" #include "../Reporting.hpp" - +#include "ReservationManager.hpp" #include namespace rmf_fleet_adapter { @@ -714,6 +714,12 @@ class RobotContext /// Release a door. This should only be used by DoorClose void _release_door(const std::string& door_name); + + void _set_allocated_destination( + const rmf_chope_msgs::msg::ReservationAllocation&); + + std::optional _release_resource(); + template static std::shared_ptr make(Args&&... args) { @@ -875,6 +881,7 @@ class RobotContext rmf_rxcpp::subscription_guard _mutex_group_sanity_check; std::chrono::steady_clock::time_point _last_active_task_time; uint64_t _last_reservation_request_id; + ReservationManager _reservation_mgr; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index e8127f8ef..91ef1c2db 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -30,6 +30,7 @@ #include "../agv/internal_EasyFullControl.hpp" #include +#include namespace rmf_fleet_adapter { namespace events { @@ -841,6 +842,7 @@ std::optional ExecutePlan::make( std::vector waypoints = plan.get_waypoints(); + auto waypoint_copy = waypoints; std::vector move_through; std::optional current_mutex_groups; std::unordered_set remaining_mutex_groups; @@ -1294,6 +1296,25 @@ std::optional ExecutePlan::make( rmf_task_sequence::events::Bundle::Type::Sequence, standbys, state, std::move(update))->begin([]() {}, std::move(finished)); + if (waypoint_copy.size() > 0 && context->location().size()) { + if (waypoint_copy.back().graph_index() != context->location()[0].waypoint()) + { + rmf_chope_msgs::msg::ReleaseRequest msg; + std::stringstream ss; + ss << "Robot "; + ss << context->name(); + ss << "Waypoints "; + ss << waypoint_copy.size(); + std::string topic = ss.str(); + msg.location = topic; + context->node()->release_location()->publish(msg); + RCLCPP_ERROR( + context->node()->get_logger(), + "Releasing waypoint number of waypoints: %lu", + waypoint_copy.size() + ); + } + } return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 02a08618b..ac758bba9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace rmf_fleet_adapter { namespace events { @@ -170,10 +171,15 @@ auto GoToPlace::Active::make( active->_reservation_ticket = active->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker(active->_context->worker())) .subscribe([w = active->weak_from_this()](const std::shared_ptr& msg) { + const auto self = w.lock(); if (!self) return; + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Got ticket issueing claim"); + if (msg->header.request_id != self->_reservation_id || msg->header.robot_name != self->_context->name() || msg->header.fleet_name != self->_context->group()) @@ -185,46 +191,49 @@ auto GoToPlace::Active::make( // Pick the nearest location to wait - auto current_location = _context->location(); + auto current_location = self->_context->location(); if (current_location.size() == 0) { return; } - // Order wait points by the number of - std::map waitpoints_order; - for (std::size_t i = 0; i < self->_context->navigation_graph()->num_waypoints(); i++) { - const auto wp = self->_context->navigation_graph()->get_waypoint(i); + // Order wait points by the distance from the destination. + std::vector> waitpoints_order; + for (std::size_t wp_idx = 0; wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) { + const auto wp = self->_context->navigation_graph().get_waypoint(wp_idx); // Wait at parking spot and check its on same floor. - if (!wp.is_parking_spot() || wp.get_map_name() != self->_context->map) { + if (!wp.is_parking_spot() || wp.get_map_name() != self->_context->map()) { continue; } - auto result = self->_context->planner()->quickest_path(current_location, wp_idx); if (!result.has_value()) { continue; } - - stringstream json_stream; - json_stream << i << std::endl; - string json; + std::stringstream json_stream; + json_stream << wp_idx << std::endl; + std::string json; json_stream >> json; - waitpoints_order.insert(result, json); + waitpoints_order.emplace_back(result->cost(), json); } - std::vector waitpoints; - for (auto &[_, name]: waitpoints_order) { - waitpoints.push_back(name); - } + std::sort(waitpoints_order.begin(), waitpoints_order.end(), [](const std::pair& a, const std::pair& b) { + return a.first < b.first; + }); // Immediately make claim cause we don't yet support flexible reservations. rmf_chope_msgs::msg::ClaimRequest claim_request; - claim_request->ticket = *msg; - claim_request->wait_points = waitpoints; + claim_request.ticket = *msg; + std::vector waitpoints; + for (auto &[_, waitpoint]: waitpoints_order) { + claim_request.wait_points.push_back(waitpoint); + } self->_context->node()->claim_location_ticket()->publish(claim_request); + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Claim issued"); }); active->_reservation_allocation = active->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker(active->_context->worker())) @@ -243,7 +252,9 @@ auto GoToPlace::Active::make( { return; } - + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Got result "); self->_final_allocated_destination = msg; }); @@ -551,6 +562,25 @@ std::optional GoToPlace::Active::_choose_goal( selected_idx = i; lowest_cost = result->cost(); } + + std::stringstream json_stream; + json_stream << wp_idx << std::endl; + std::string json; + json_stream >> json; + + rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; + alternative.resource_name = json; + alternative.cost = result->cost(); + alternative.has_end = false; + + rmf_chope_msgs::msg::StartTimeRange start; + start.earliest_start_time = _context->node()->get_clock()->now(); + start.latest_start_time = start.earliest_start_time; + start.has_earliest_start_time = true; + start.has_latest_start_time = true; + alternative.start_time = start; + + ftr.alternatives.push_back(alternative); } else { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp new file mode 100644 index 000000000..a877334d6 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2020 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 "ReleaseResource.hpp" +#include "RxOperators.hpp" +#include "SupervisorHasSession.hpp" +#include "rmf_fleet_adapter/StandardNames.hpp" + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +std::shared_ptr ReleaseResource::ActivePhase::make( + agv::RobotContextPtr context, + std::string door_name, + std::string request_id) +{ + RCLCPP_INFO( + context->node()->get_logger(), + "Releasing [%s] for [%s]", + door_name.c_str(), + context->requester_id().c_str()); + context->_release_door(door_name); + auto inst = std::shared_ptr(new ActivePhase( + std::move(context), + std::move(door_name), + std::move(request_id) + )); + inst->_init_obs(); + return inst; +} + +//============================================================================== +const rxcpp::observable& +ReleaseResource::ActivePhase::observe() const +{ + return _obs; +} + +//============================================================================== +rmf_traffic::Duration ReleaseResource::ActivePhase::estimate_remaining_time() const +{ + // TODO: implement + return rmf_traffic::Duration{0}; +} + +//============================================================================== +void ReleaseResource::ActivePhase::emergency_alarm(bool /*on*/) +{ + // TODO: implement +} + +//============================================================================== +void ReleaseResource::ActivePhase::cancel() +{ + // Don't actually cancel anything here. We just release the space. +} + +//============================================================================== +const std::string& ReleaseResource::ActivePhase::description() const +{ + return _description; +} + +//============================================================================== +void ReleaseResource::ActivePhase::_init_obs() +{ + +} + +//============================================================================== +void ReleaseResource::ActivePhase::_publish_release_spot() +{ +} + +//============================================================================== +ReleaseResource::ActivePhase::ActivePhase( + agv::RobotContextPtr context, + std::string door_name, + std::string request_id) +: _context(std::move(context)), + _door_name(std::move(door_name)), + _request_id(std::move(request_id)) +{ + _description = "Closing [door:" + _door_name + "]"; +} + +//============================================================================== +ReleaseResource::PendingPhase::PendingPhase( + agv::RobotContextPtr context) +: _context(std::move(context)) +{ + _description = "Releasing Resource"; +} + +//============================================================================== +std::shared_ptr ReleaseResource::PendingPhase::begin() +{ + return ReleaseResource::ActivePhase::make( + _context, + _door_name, + _request_id); +} + +//============================================================================== +rmf_traffic::Duration ReleaseResource::PendingPhase::estimate_phase_duration() const +{ + // TODO: implement + return rmf_traffic::Duration{0}; +} + +//============================================================================== +const std::string& ReleaseResource::PendingPhase::description() const +{ + return _description; +} + +} // namespace phases +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp new file mode 100644 index 000000000..4c7865628 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2020 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 SRC__RMF_FLEET_ADAPTER__PHASES__RELEASE_RESOURCE_HPP +#define SRC__RMF_FLEET_ADAPTER__PHASES__RELEASE_RESOURCE_HPP + +#include "../LegacyTask.hpp" +#include "../agv/RobotContext.hpp" + +#include +#include + +namespace rmf_fleet_adapter { +namespace phases { + +struct ReleaseResource +{ + /** + * The phase should do the following + * 1. Send out a MODE_CLOSED door request + * 2. Periodically resend the close request while the supervisor state contains the requester_id + * 3. It is completed when the supervisor state does NOT contains the requester_id, regardless of the door state + * 4. Cancellation requests are ignored + */ + class ActivePhase : public LegacyTask::ActivePhase, + public std::enable_shared_from_this + { + public: + + static std::shared_ptr make( + agv::RobotContextPtr context, + std::string door_name, + std::string request_id); + + const rxcpp::observable& observe() const override; + + rmf_traffic::Duration estimate_remaining_time() const override; + + void emergency_alarm(bool on) override; + + void cancel() override; + + const std::string& description() const override; + + private: + + agv::RobotContextPtr _context; + std::string _door_name; + std::string _request_id; + rxcpp::observable _obs; + std::string _description; + rclcpp::TimerBase::SharedPtr _timer; + LegacyTask::StatusMsg _status; + + ActivePhase( + agv::RobotContextPtr context, + std::string door_name, + std::string request_id); + + void _init_obs(); + + void _publish_release_spot(); + }; + + class PendingPhase : public LegacyTask::PendingPhase + { + public: + + PendingPhase( + agv::RobotContextPtr context); + + std::shared_ptr begin() override; + + rmf_traffic::Duration estimate_phase_duration() const override; + + const std::string& description() const override; + + private: + + agv::RobotContextPtr _context; + std::string _door_name; + std::string _request_id; + std::string _description; + }; +}; + +} // namespace phases +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__PHASES__DOORCLOSE_HPP From e1c37fc577fddda738325557e8faac58d266ef4f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 1 Feb 2024 16:18:06 +0800 Subject: [PATCH 017/119] Added a simple C++ mutex based proto-reservation system Removed all the rust code. This is a stop-gap measure for legacy RMF and should prevent erroneous occupation of parking spots. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/CMakeLists.txt | 37 + rmf_chope_node/package.xml | 21 + rmf_chope_node/src/main.cpp | 287 ++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 30 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 161 +- .../rmf_fleet_adapter/events/GoToPlace.hpp | 10 +- rmf_reservation_ros2/Cargo.lock | 1364 ----------------- rmf_reservation_ros2/Cargo.toml | 27 - rmf_reservation_ros2/package.xml | 24 - rmf_reservation_ros2/src/reservation_node.rs | 172 --- 10 files changed, 463 insertions(+), 1670 deletions(-) create mode 100644 rmf_chope_node/CMakeLists.txt create mode 100644 rmf_chope_node/package.xml create mode 100644 rmf_chope_node/src/main.cpp delete mode 100644 rmf_reservation_ros2/Cargo.lock delete mode 100644 rmf_reservation_ros2/Cargo.toml delete mode 100644 rmf_reservation_ros2/package.xml delete mode 100644 rmf_reservation_ros2/src/reservation_node.rs diff --git a/rmf_chope_node/CMakeLists.txt b/rmf_chope_node/CMakeLists.txt new file mode 100644 index 000000000..14abc0ad0 --- /dev/null +++ b/rmf_chope_node/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(rmf_chope_node) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmf_chope_msgs REQUIRED) +find_package(rmf_fleet_adapter REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(queue_manager src/main.cpp) +ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs) + +install(TARGETS + queue_manager + DESTINATION lib/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/rmf_chope_node/package.xml b/rmf_chope_node/package.xml new file mode 100644 index 000000000..f7589fc52 --- /dev/null +++ b/rmf_chope_node/package.xml @@ -0,0 +1,21 @@ + + + + rmf_chope_node + 0.0.0 + TODO: Package description + arjoc + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rmf_chope_msgs + rmf_fleet_adapter + + + ament_cmake + + diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp new file mode 100644 index 000000000..52deaf975 --- /dev/null +++ b/rmf_chope_node/src/main.cpp @@ -0,0 +1,287 @@ +#include "rmf_chope_msgs/msg/flexible_time_request.hpp" +#include "rmf_chope_msgs/msg/claim_request.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +const std::string ReservationRequestTopicName = "/rmf/reservations/request"; +const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; +const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; +const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; +const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; + + +/// C++-isms +template <> +struct std::hash +{ + std::size_t operator()(const rmf_chope_msgs::msg::RequestHeader& header) const + { + using std::size_t; + using std::hash; + using std::string; + + + return ((hash()(header.robot_name) + ^ (hash()(header.fleet_name) << 1)) >> 1) + ^ (hash()(header.request_id) << 1); + } +}; + + +/// Useful for identifying which tickets belong to which robots. +struct RobotIdentifier { + std::string robot_name; + std::string fleet_name; + + bool operator==(const RobotIdentifier& other) const { + return robot_name == other.robot_name && fleet_name == other.fleet_name; + } +}; + + +template <> +struct std::hash +{ + std::size_t operator()(const RobotIdentifier& header) const + { + using std::size_t; + using std::hash; + using std::string; + + + return ((hash()(header.robot_name) + ^ (hash()(header.fleet_name) << 1)) >> 1); + } +}; + + +/// Ticket generation class for book keeping purposes. Will eventually overflow and leak memory. +/// Ticket id 0 does not exist and is useful for making emergency claims. +class TicketStore { + +public: + rmf_chope_msgs::msg::Ticket get_new_ticket(const rmf_chope_msgs::msg::RequestHeader &request_header) + { + rmf_chope_msgs::msg::Ticket ticket; + ticket.header = request_header; + ticket.ticket_id = _last_issued_ticket_id; + RobotIdentifier robot_identifier { + request_header.robot_name, + request_header.fleet_name + }; + _robots_to_tickets[robot_identifier].push_back(_last_issued_ticket_id); + _header_to_ticket.emplace(request_header, _last_issued_ticket_id); + _ticket_to_header.emplace(_last_issued_ticket_id, request_header); + _last_issued_ticket_id++; + return ticket; + } + + rmf_chope_msgs::msg::Ticket get_existing_ticket(const std::size_t index) + { + rmf_chope_msgs::msg::Ticket ticket; + ticket.ticket_id = index; + ticket.header = _ticket_to_header[index]; + return ticket; + } + + std::unordered_map> _robots_to_tickets; + std::unordered_map _header_to_ticket; + std::unordered_map _ticket_to_header; + std::size_t _last_issued_ticket_id = 1; +}; + +struct LocationState { + std::optional ticket; +}; + +struct LocationReq { + std::string location; + double cost; + + bool operator<(const LocationReq& other) const { + return cost < other.cost; + } + + bool operator==(const LocationReq& other) const { + return cost == other.cost && location == other.location; + } +}; + + +/// Implements a simple Mutex. Only one robot can claim a location at a time. +/// The current implementation is relatively simplistic and basically checks if a location is occupied or not. +/// A queuing system is in the works. Note: It is possible for the current system to get deadlocked. +class CurrentState { +public: + std::vector free_locations() { + std::vector locations; + for (auto &[loc, state] : _current_location_reservations) { + if (!state.ticket.has_value()) { + locations.push_back(loc); + } + } + return locations; + } + + std::optional allocate_lowest_cost_free_spot(const std::vector& incoming_requests, const std::size_t ticket_id) + { + auto requests = incoming_requests; + std::sort(requests.begin(), requests.end()); + for (std::size_t i = 0; i < requests.size(); i++) { + auto parking = _current_location_reservations.find(requests[i].location); + if (parking == _current_location_reservations.end()) { + _current_location_reservations.emplace(requests[i].location, LocationState {ticket_id}); + _ticket_to_location.emplace(ticket_id, requests[i].location); + return requests[i].location; + } + else if (!parking->second.ticket.has_value()){ + _current_location_reservations.emplace(parking->first, LocationState {ticket_id}); + _ticket_to_location.emplace(ticket_id, requests[i].location); + return parking->first; + } + } + + return std::nullopt; + } + + bool release(const std::size_t ticket_id) + { + auto _ticket = _ticket_to_location.find(ticket_id); + if (_ticket == _ticket_to_location.end()) + { + return false; + } + auto location = _ticket->second; + _current_location_reservations[location].ticket = std::nullopt; + return true; + } + +private: + std::unordered_map _current_location_reservations; + std::unordered_map _ticket_to_location; +}; + +class SimpleQueueSystemNode : public rclcpp::Node { +public: + SimpleQueueSystemNode() : Node("rmf_chope_node") { + + rclcpp::QoS qos(10); + qos = qos.reliable(); + qos = qos.keep_last(10); + qos = qos.transient_local(); + + request_subscription_ = this->create_subscription( + ReservationRequestTopicName, qos, std::bind(&SimpleQueueSystemNode::on_request, this, std::placeholders::_1)); + claim_subscription_ = this->create_subscription( + ReservationClaimTopicName, qos, std::bind(&SimpleQueueSystemNode::claim_request, this, std::placeholders::_1)); + release_subscription_ = this->create_subscription( + ReservationReleaseTopicName, qos, std::bind(&SimpleQueueSystemNode::release, this, std::placeholders::_1)); + + ticket_pub_ = this->create_publisher(ReservationResponseTopicName, qos); + allocation_pub_ = this->create_publisher(ReservationAllocationTopicName, qos); + } + +private: + void on_request(const rmf_chope_msgs::msg::FlexibleTimeRequest::ConstSharedPtr &request) { + + std::vector requests; + + for (auto alt: request->alternatives) { + LocationReq request{ + alt.resource_name, + alt.cost + }; + + requests.push_back(request); + } + + auto ticket = ticket_store_.get_new_ticket(request->header); + requests_[ticket.ticket_id] = requests; + ticket_pub_->publish(ticket); + } + + void claim_request(const rmf_chope_msgs::msg::ClaimRequest::ConstSharedPtr &request) { + + // This logic is for the simplified queue-less version. + std::vector locations; + + for (auto claim: requests_[request->ticket.ticket_id]) + { + locations.push_back(claim); + } + + auto cost = (locations.size() == 0) ? 0.0: locations.back().cost + 1.0; + for (auto claim: request->wait_points) { + locations.push_back(LocationReq { + claim, + cost + }); + cost += 1.0; + } + auto result = current_state_.allocate_lowest_cost_free_spot(locations, request->ticket.ticket_id); + if (result.has_value()) + { + rmf_chope_msgs::msg::ReservationAllocation allocation; + allocation.ticket = ticket_store_.get_existing_ticket(request->ticket.ticket_id); + allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + + allocation.resource = result.value(); + + for (std::size_t i=0; i < requests_[request->ticket.ticket_id].size(); i++) + { + if (requests_[request->ticket.ticket_id][i].location == result.value()) + { + allocation.satisfies_alternative = i; + } + } + + allocation_pub_->publish(allocation); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Could not allocate resource for ticket %lu.", request->ticket.ticket_id); + } + } + + void release(const rmf_chope_msgs::msg::ReleaseRequest::ConstSharedPtr &request) { + RCLCPP_INFO(this->get_logger(), "Releasing ticket for %lu", request->ticket.ticket_id); + auto ticket = request->ticket.ticket_id; + auto success = current_state_.release(ticket); + if (!success) { + RCLCPP_ERROR(this->get_logger(), "Could not find ticker %lu", request->ticket.ticket_id); + } + } + + rclcpp::Subscription::SharedPtr request_subscription_; + rclcpp::Subscription::SharedPtr claim_subscription_; + rclcpp::Subscription::SharedPtr release_subscription_; + + rclcpp::Publisher::SharedPtr ticket_pub_; + rclcpp::Publisher::SharedPtr allocation_pub_; + + std::unordered_map> requests_; + TicketStore ticket_store_; + CurrentState current_state_; +}; + +int main (int argc, const char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 91ef1c2db..4d39d763a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -1299,20 +1299,22 @@ std::optional ExecutePlan::make( if (waypoint_copy.size() > 0 && context->location().size()) { if (waypoint_copy.back().graph_index() != context->location()[0].waypoint()) { - rmf_chope_msgs::msg::ReleaseRequest msg; - std::stringstream ss; - ss << "Robot "; - ss << context->name(); - ss << "Waypoints "; - ss << waypoint_copy.size(); - std::string topic = ss.str(); - msg.location = topic; - context->node()->release_location()->publish(msg); - RCLCPP_ERROR( - context->node()->get_logger(), - "Releasing waypoint number of waypoints: %lu", - waypoint_copy.size() - ); + auto allocation = context->_release_resource(); + if (allocation.has_value()) + { + rmf_chope_msgs::msg::ReleaseRequest msg; + std::stringstream str; + str << context->location()[0].waypoint(); + str >> msg.location; + msg.ticket = allocation->ticket; + context->node()->release_location()->publish(msg); + RCLCPP_ERROR( + context->node()->get_logger(), + "Releasing waypoint", + waypoint_copy.size() + ); + } + } } return ExecutePlan{ diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index ac758bba9..8232a4682 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -252,10 +253,9 @@ auto GoToPlace::Active::make( { return; } - RCLCPP_ERROR( - self->_context->node()->get_logger(), - "Got result "); + self->_final_allocated_destination = msg; + self->_current_reservation_state = ReservationState::RecievedResponse; }); @@ -498,7 +498,7 @@ std::string wp_name(const agv::RobotContext& context) //============================================================================== std::optional GoToPlace::Active::_choose_goal( - bool only_same_map) const + bool only_same_map) { auto current_location = _context->location(); auto graph = _context->navigation_graph(); @@ -518,85 +518,110 @@ std::optional GoToPlace::Active::_choose_goal( _description.one_of().size(), _context->requester_id().c_str()); - // Select node - rmf_chope_msgs::msg::FlexibleTimeRequest ftr; - ftr.header.robot_name = _context->name(); - ftr.header.fleet_name = _context->group(); - ftr.header.request_id = _reservation_id; - auto lowest_cost = std::numeric_limits::infinity(); - std::optional selected_idx; - for (std::size_t i = 0; i < _description.one_of().size(); ++i) + // No need to use reservation system if we are already there. + if (_description.one_of().size() == 1 + && _description.one_of()[0].waypoint() == current_location[0].waypoint()) { - const auto wp_idx = _description.one_of()[i].waypoint(); - if (only_same_map) + return _description.one_of()[0]; + } + + + if (_current_reservation_state == ReservationState::Pending) + { + // Select node + rmf_chope_msgs::msg::FlexibleTimeRequest ftr; + ftr.header.robot_name = _context->name(); + ftr.header.fleet_name = _context->group(); + ftr.header.request_id = _reservation_id; + + auto lowest_cost = std::numeric_limits::infinity(); + std::optional selected_idx; + for (std::size_t i = 0; i < _description.one_of().size(); ++i) { - const auto& wp = graph.get_waypoint(wp_idx); + const auto wp_idx = _description.one_of()[i].waypoint(); + if (only_same_map) + { + const auto& wp = graph.get_waypoint(wp_idx); + + // Check if same map. If not don't consider location. This is to ensure + // the robot does not try to board a lift. + if (wp.get_map_name() != _context->map()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + _context->map().c_str()); + continue; + } + } - // Check if same map. If not don't consider location. This is to ensure - // the robot does not try to board a lift. - if (wp.get_map_name() != _context->map()) + // Find distance to said point + auto result = _context->planner()->quickest_path(current_location, wp_idx); + if (result.has_value()) { RCLCPP_INFO( _context->node()->get_logger(), - "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + "Got distance from [%lu] as %f", wp_idx, - wp.get_map_name().c_str(), - _context->map().c_str()); - continue; - } - } + result->cost()); - // Find distance to said point - auto result = _context->planner()->quickest_path(current_location, wp_idx); - if (result.has_value()) - { - RCLCPP_INFO( - _context->node()->get_logger(), - "Got distance from [%lu] as %f", - wp_idx, - result->cost()); + if (result->cost() < lowest_cost) + { + selected_idx = i; + lowest_cost = result->cost(); + } - if (result->cost() < lowest_cost) + std::stringstream json_stream; + json_stream << wp_idx << std::endl; + std::string json; + json_stream >> json; + + rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; + alternative.resource_name = json; + alternative.cost = result->cost(); + alternative.has_end = false; + + rmf_chope_msgs::msg::StartTimeRange start; + start.earliest_start_time = _context->node()->get_clock()->now(); + start.latest_start_time = start.earliest_start_time; + start.has_earliest_start_time = true; + start.has_latest_start_time = true; + alternative.start_time = start; + + ftr.alternatives.push_back(alternative); + } + else { - selected_idx = i; - lowest_cost = result->cost(); + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + wp_idx, + _context->requester_id().c_str()); } - - std::stringstream json_stream; - json_stream << wp_idx << std::endl; - std::string json; - json_stream >> json; - - rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; - alternative.resource_name = json; - alternative.cost = result->cost(); - alternative.has_end = false; - - rmf_chope_msgs::msg::StartTimeRange start; - start.earliest_start_time = _context->node()->get_clock()->now(); - start.latest_start_time = start.earliest_start_time; - start.has_earliest_start_time = true; - start.has_latest_start_time = true; - alternative.start_time = start; - - ftr.alternatives.push_back(alternative); } - else - { - RCLCPP_ERROR( - _context->node()->get_logger(), - "No path found for robot [%s] to waypoint [%lu]", - wp_idx, - _context->requester_id().c_str()); + _context->node()->location_requester()->publish(ftr); + _current_reservation_state = ReservationState::Requested; + } + else if (_current_reservation_state == ReservationState::RecievedResponse) + { + // Go to recommended destination + if (_final_allocated_destination.value()->instruction_type == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { + _context->_set_allocated_destination(*_final_allocated_destination.value().get()); + return _description.one_of()[_final_allocated_destination.value()->satisfies_alternative]; } - } - _context->node()->location_requester()->publish(ftr); - + // For queueing system in future + //return rmf_traffic::agv::Plan::Goal(std::stoul(_final_allocated_destination.value()->resource), std::nullopt); - if (selected_idx.has_value()) - { - return _description.one_of()[*selected_idx]; + // For now just keep retrying until your turn. + // Probably not needed since, the node does not publish an allocation till its available? + RCLCPP_ERROR( + _context->node()->get_logger(), + "Unable to immediately service call, awaiting more." + ); + return std::nullopt; } return std::nullopt; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 2c5081cab..fd0a31a02 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -106,12 +106,20 @@ class GoToPlace : public rmf_task_sequence::Event private: + enum class ReservationState { + Pending=0, + Requested=1, + RecievedResponse=2 + }; + + ReservationState _current_reservation_state = ReservationState::Pending; + Active(Description description); void _schedule_retry(); std::optional _choose_goal( - bool only_same_map) const; + bool only_same_map); void _find_plan(); diff --git a/rmf_reservation_ros2/Cargo.lock b/rmf_reservation_ros2/Cargo.lock deleted file mode 100644 index 944dac430..000000000 --- a/rmf_reservation_ros2/Cargo.lock +++ /dev/null @@ -1,1364 +0,0 @@ -# This file is automatically @generated by Cargo. -# It is not intended for manual editing. -version = 3 - -[[package]] -name = "addr2line" -version = "0.21.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8a30b2e23b9e17a9f90641c7ab1549cd9b44f296d3ccbf309d2863cfe398a0cb" -dependencies = [ - "gimli", -] - -[[package]] -name = "adler" -version = "1.0.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f26201604c87b1e01bd3d98f8d5d9a8fcbb815e8cedb41ffccbeb4bf593a35fe" - -[[package]] -name = "aho-corasick" -version = "1.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b2969dcb958b36655471fc61f7e416fa76033bdd4bfed0678d8fee1e2d07a1f0" -dependencies = [ - "memchr", -] - -[[package]] -name = "android-tzdata" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" - -[[package]] -name = "android_system_properties" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" -dependencies = [ - "libc", -] - -[[package]] -name = "anyhow" -version = "1.0.79" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "080e9890a082662b09c1ad45f567faeeb47f22b5fb23895fbe1e651e718e25ca" -dependencies = [ - "backtrace", -] - -[[package]] -name = "autocfg" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" - -[[package]] -name = "backtrace" -version = "0.3.69" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2089b7e3f35b9dd2d0ed921ead4f6d318c27680d4a5bd167b3ee120edb105837" -dependencies = [ - "addr2line", - "cc", - "cfg-if", - "libc", - "miniz_oxide", - "object", - "rustc-demangle", -] - -[[package]] -name = "bindgen" -version = "0.66.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f2b84e06fc203107bfbad243f4aba2af864eb7db3b1cf46ea0a023b0b433d2a7" -dependencies = [ - "bitflags", - "cexpr", - "clang-sys", - "lazy_static", - "lazycell", - "log", - "peeking_take_while", - "prettyplease", - "proc-macro2", - "quote", - "regex", - "rustc-hash", - "shlex", - "syn 2.0.48", - "which", -] - -[[package]] -name = "bitflags" -version = "2.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07" - -[[package]] -name = "builtin_interfaces" -version = "1.2.1" -dependencies = [ - "rosidl_runtime_rs", -] - -[[package]] -name = "bumpalo" -version = "3.14.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f30e7476521f6f8af1a1c4c0b8cc94f0bee37d91763d0ca2665f299b6cd8aec" - -[[package]] -name = "cc" -version = "1.0.83" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1174fb0b6ec23863f8b971027804a42614e347eafb0a95bf0b12cdae21fc4d0" -dependencies = [ - "libc", -] - -[[package]] -name = "cexpr" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" -dependencies = [ - "nom", -] - -[[package]] -name = "cfg-if" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" - -[[package]] -name = "chrono" -version = "0.4.31" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f2c685bad3eb3d45a01354cedb7d5faa66194d1d58ba6e267a8de788f79db38" -dependencies = [ - "android-tzdata", - "iana-time-zone", - "js-sys", - "num-traits", - "pure-rust-locales", - "rustc-serialize", - "serde", - "wasm-bindgen", - "windows-targets 0.48.5", -] - -[[package]] -name = "clang-sys" -version = "1.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" -dependencies = [ - "glob", - "libc", - "libloading", -] - -[[package]] -name = "core-foundation-sys" -version = "0.8.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "06ea2b9bc92be3c2baa9334a323ebca2d6f074ff852cd1d7b11064035cd3868f" - -[[package]] -name = "deprecate-until" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7a3767f826efbbe5a5ae093920b58b43b01734202be697e1354914e862e8e704" -dependencies = [ - "proc-macro2", - "quote", - "semver", - "syn 2.0.48", -] - -[[package]] -name = "either" -version = "1.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a26ae43d7bcc3b814de94796a5e736d4029efb0ee900c12e2d54c993ad1a1e07" - -[[package]] -name = "equivalent" -version = "1.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" - -[[package]] -name = "errno" -version = "0.3.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" -dependencies = [ - "libc", - "windows-sys 0.52.0", -] - -[[package]] -name = "fixedbitset" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80" - -[[package]] -name = "fnv" -version = "1.0.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" - -[[package]] -name = "futures" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0" -dependencies = [ - "futures-channel", - "futures-core", - "futures-executor", - "futures-io", - "futures-sink", - "futures-task", - "futures-util", -] - -[[package]] -name = "futures-channel" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" -dependencies = [ - "futures-core", - "futures-sink", -] - -[[package]] -name = "futures-core" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" - -[[package]] -name = "futures-executor" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a576fc72ae164fca6b9db127eaa9a9dda0d61316034f33a0a0d4eda41f02b01d" -dependencies = [ - "futures-core", - "futures-task", - "futures-util", -] - -[[package]] -name = "futures-io" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1" - -[[package]] -name = "futures-macro" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.48", -] - -[[package]] -name = "futures-sink" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" - -[[package]] -name = "futures-task" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" - -[[package]] -name = "futures-util" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" -dependencies = [ - "futures-channel", - "futures-core", - "futures-io", - "futures-macro", - "futures-sink", - "futures-task", - "memchr", - "pin-project-lite", - "pin-utils", - "slab", -] - -[[package]] -name = "getrandom" -version = "0.2.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "190092ea657667030ac6a35e305e62fc4dd69fd98ac98631e5d3a2b1575a12b5" -dependencies = [ - "cfg-if", - "libc", - "wasi", -] - -[[package]] -name = "gimli" -version = "0.28.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4271d37baee1b8c7e4b708028c57d816cf9d2434acb33a549475f78c181f6253" - -[[package]] -name = "glob" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" - -[[package]] -name = "hashbrown" -version = "0.14.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" - -[[package]] -name = "home" -version = "0.5.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" -dependencies = [ - "windows-sys 0.52.0", -] - -[[package]] -name = "iana-time-zone" -version = "0.1.59" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b6a67363e2aa4443928ce15e57ebae94fd8949958fd1223c4cfc0cd473ad7539" -dependencies = [ - "android_system_properties", - "core-foundation-sys", - "iana-time-zone-haiku", - "js-sys", - "wasm-bindgen", - "windows-core", -] - -[[package]] -name = "iana-time-zone-haiku" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" -dependencies = [ - "cc", -] - -[[package]] -name = "indexmap" -version = "2.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d530e1a18b1cb4c484e6e34556a0d948706958449fca0cab753d649f2bce3d1f" -dependencies = [ - "equivalent", - "hashbrown", -] - -[[package]] -name = "integer-sqrt" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "276ec31bcb4a9ee45f58bec6f9ec700ae4cf4f4f8f2fa7e06cb406bd5ffdd770" -dependencies = [ - "num-traits", -] - -[[package]] -name = "iter_tools" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "531cafdc99b3b3252bb32f5620e61d56b19415efc19900b12d1b2e7483854897" -dependencies = [ - "itertools", -] - -[[package]] -name = "itertools" -version = "0.10.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" -dependencies = [ - "either", -] - -[[package]] -name = "itoa" -version = "0.4.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b71991ff56294aa922b450139ee08b3bfc70982c6b2c7562771375cf73542dd4" - -[[package]] -name = "js-sys" -version = "0.3.66" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cee9c64da59eae3b50095c18d3e74f8b73c0b86d2792824ff01bbce68ba229ca" -dependencies = [ - "wasm-bindgen", -] - -[[package]] -name = "lazy_static" -version = "1.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" - -[[package]] -name = "lazycell" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" - -[[package]] -name = "leb128" -version = "0.2.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "884e2677b40cc8c339eaefcb701c32ef1fd2493d71118dc0ca4b6a736c93bd67" - -[[package]] -name = "libc" -version = "0.2.152" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "13e3bf6590cbc649f4d1a3eefc9d5d6eb746f5200ffb04e5e142700b8faa56e7" - -[[package]] -name = "libloading" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c571b676ddfc9a8c12f1f3d3085a7b163966a8fd8098a90640953ce5f6170161" -dependencies = [ - "cfg-if", - "windows-sys 0.48.0", -] - -[[package]] -name = "linux-raw-sys" -version = "0.4.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c4cd1a83af159aa67994778be9070f0ae1bd732942279cabb14f86f986a21456" - -[[package]] -name = "log" -version = "0.4.20" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" - -[[package]] -name = "memchr" -version = "2.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" - -[[package]] -name = "minimal-lexical" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" - -[[package]] -name = "miniz_oxide" -version = "0.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e7810e0be55b428ada41041c41f32c9f1a42817901b4ccf45fa3d4b6561e74c7" -dependencies = [ - "adler", -] - -[[package]] -name = "nom" -version = "7.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" -dependencies = [ - "memchr", - "minimal-lexical", -] - -[[package]] -name = "num-traits" -version = "0.2.17" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" -dependencies = [ - "autocfg", -] - -[[package]] -name = "object" -version = "0.32.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a6a622008b6e321afc04970976f62ee297fdbaa6f95318ca343e3eebb9648441" -dependencies = [ - "memchr", -] - -[[package]] -name = "once_cell" -version = "1.19.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" - -[[package]] -name = "ordered-float" -version = "2.10.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "68f19d67e5a2795c94e73e0bb1cc1a7edeb2e28efd39e2e1c9b7a40c1108b11c" -dependencies = [ - "num-traits", -] - -[[package]] -name = "ordered-float" -version = "3.9.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1e1c390732d15f1d48471625cd92d154e66db2c56645e29a9cd26f4699f72dc" -dependencies = [ - "num-traits", -] - -[[package]] -name = "partial_ref" -version = "0.3.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f728bc9b1479656e40cba507034904a8c44027c0efdbbaf6a4bdc5f2d3a910c" -dependencies = [ - "partial_ref_derive", -] - -[[package]] -name = "partial_ref_derive" -version = "0.3.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "300e1d2cb5b898b5a5342e994e0d0c367dbfe69cbf717cd307045ec9fb057581" -dependencies = [ - "proc-macro2", - "quote", - "syn 1.0.109", -] - -[[package]] -name = "pathfinding" -version = "4.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b425bfb8934d227558811f4b8c94d6fe64c9069c1387deed0f586bc418271b2" -dependencies = [ - "deprecate-until", - "fixedbitset", - "indexmap", - "integer-sqrt", - "num-traits", - "rustc-hash", - "thiserror", -] - -[[package]] -name = "peeking_take_while" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" - -[[package]] -name = "petgraph" -version = "0.6.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e1d3afd2628e69da2be385eb6f2fd57c8ac7977ceeff6dc166ff1657b0e386a9" -dependencies = [ - "fixedbitset", - "indexmap", -] - -[[package]] -name = "pin-project-lite" -version = "0.2.13" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8afb450f006bf6385ca15ef45d71d2288452bc3683ce2e2cacc0d18e4be60b58" - -[[package]] -name = "pin-utils" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" - -[[package]] -name = "ppv-lite86" -version = "0.2.17" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" - -[[package]] -name = "prettyplease" -version = "0.2.16" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a41cf62165e97c7f814d2221421dbb9afcbcdb0a88068e5ea206e19951c2cbb5" -dependencies = [ - "proc-macro2", - "syn 2.0.48", -] - -[[package]] -name = "proc-macro2" -version = "1.0.76" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95fc56cda0b5c3325f5fbbd7ff9fda9e02bb00bb3dac51252d2f1bfa1cb8cc8c" -dependencies = [ - "unicode-ident", -] - -[[package]] -name = "pure-rust-locales" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed02a829e62dc2715ceb8afb4f80e298148e1345749ceb369540fe0eb3368432" - -[[package]] -name = "quote" -version = "1.0.35" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" -dependencies = [ - "proc-macro2", -] - -[[package]] -name = "rand" -version = "0.8.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" -dependencies = [ - "libc", - "rand_chacha", - "rand_core", -] - -[[package]] -name = "rand_chacha" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" -dependencies = [ - "ppv-lite86", - "rand_core", -] - -[[package]] -name = "rand_core" -version = "0.6.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" -dependencies = [ - "getrandom", -] - -[[package]] -name = "rand_pcg" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "59cad018caf63deb318e5a4586d99a24424a364f40f1e5778c29aca23f4fc73e" -dependencies = [ - "rand_core", -] - -[[package]] -name = "rand_seeder" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf2890aaef0aa82719a50e808de264f9484b74b442e1a3a0e5ee38243ac40bdb" -dependencies = [ - "rand_core", -] - -[[package]] -name = "rclrs" -version = "0.4.1" -dependencies = [ - "bindgen", - "builtin_interfaces", - "cfg-if", - "futures", - "rosidl_runtime_rs", -] - -[[package]] -name = "regex" -version = "1.10.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "380b951a9c5e80ddfd6136919eef32310721aa4aacd4889a8d39124b026ab343" -dependencies = [ - "aho-corasick", - "memchr", - "regex-automata", - "regex-syntax", -] - -[[package]] -name = "regex-automata" -version = "0.4.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f804c7828047e88b2d32e2d7fe5a105da8ee3264f01902f796c8e067dc2483f" -dependencies = [ - "aho-corasick", - "memchr", - "regex-syntax", -] - -[[package]] -name = "regex-syntax" -version = "0.8.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f" - -[[package]] -name = "rmf_chope_msgs" -version = "0.0.0" -dependencies = [ - "builtin_interfaces", - "rosidl_runtime_rs", -] - -[[package]] -name = "rmf_reservation_ros2" -version = "0.3.1" -dependencies = [ - "anyhow", - "builtin_interfaces", - "chrono", - "rclrs", - "rmf_chope_msgs", - "rmf_reservations", - "rosidl_runtime_rs", -] - -[[package]] -name = "rmf_reservations" -version = "0.1.0" -source = "git+https://github.com/open-rmf/rmf_reservation?rev=41f01f8#41f01f86033e46e0aa7003318d8b037241c183af" -dependencies = [ - "chrono", - "fnv", - "futures", - "iter_tools", - "itertools", - "ordered-float 3.9.2", - "pathfinding", - "petgraph", - "rand", - "rand_pcg", - "rand_seeder", - "serde", - "serde_derive", - "term-table", - "uuid", - "varisat", -] - -[[package]] -name = "rosidl_runtime_rs" -version = "0.4.1" -dependencies = [ - "cfg-if", -] - -[[package]] -name = "rustc-demangle" -version = "0.1.23" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d626bb9dae77e28219937af045c257c28bfd3f69333c512553507f5f9798cb76" - -[[package]] -name = "rustc-hash" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" - -[[package]] -name = "rustc-serialize" -version = "0.3.25" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fe834bc780604f4674073badbad26d7219cadfb4a2275802db12cbae17498401" - -[[package]] -name = "rustix" -version = "0.38.28" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72e572a5e8ca657d7366229cdde4bd14c4eb5499a9573d4d366fe1b599daa316" -dependencies = [ - "bitflags", - "errno", - "libc", - "linux-raw-sys", - "windows-sys 0.52.0", -] - -[[package]] -name = "semver" -version = "1.0.21" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b97ed7a9823b74f99c7742f5336af7be5ecd3eeafcb1507d1fa93347b1d589b0" - -[[package]] -name = "serde" -version = "1.0.195" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "63261df402c67811e9ac6def069e4786148c4563f4b50fd4bf30aa370d626b02" -dependencies = [ - "serde_derive", -] - -[[package]] -name = "serde_derive" -version = "1.0.195" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46fe8f8603d81ba86327b23a2e9cdf49e1255fb94a4c5f297f6ee0547178ea2c" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.48", -] - -[[package]] -name = "shlex" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7cee0529a6d40f580e7a5e6c495c8fbfe21b7b52795ed4bb5e62cdf92bc6380" - -[[package]] -name = "slab" -version = "0.4.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67" -dependencies = [ - "autocfg", -] - -[[package]] -name = "smallvec" -version = "1.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4dccd0940a2dcdf68d092b8cbab7dc0ad8fa938bf95787e1b916b0e3d0e8e970" - -[[package]] -name = "syn" -version = "1.0.109" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "syn" -version = "2.0.48" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f3531638e407dfc0814761abb7c00a5b54992b849452a0646b7f65c9f770f3f" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "synstructure" -version = "0.12.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f36bdaa60a83aca3921b5259d5400cbf5e90fc51931376a9bd4a0eb79aa7210f" -dependencies = [ - "proc-macro2", - "quote", - "syn 1.0.109", - "unicode-xid", -] - -[[package]] -name = "term-table" -version = "1.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d5e59d7fb313157de2a568be8d81e4d7f9af6e50e697702e8e00190a6566d3b8" -dependencies = [ - "lazy_static", - "regex", - "unicode-width", -] - -[[package]] -name = "thiserror" -version = "1.0.56" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d54378c645627613241d077a3a79db965db602882668f9136ac42af9ecb730ad" -dependencies = [ - "thiserror-impl", -] - -[[package]] -name = "thiserror-impl" -version = "1.0.56" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa0faa943b50f3db30a20aa7e265dbc66076993efed8463e8de414e5d06d3471" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.48", -] - -[[package]] -name = "unicode-ident" -version = "1.0.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" - -[[package]] -name = "unicode-width" -version = "0.1.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e51733f11c9c4f72aa0c160008246859e340b00807569a0da0e7a1079b27ba85" - -[[package]] -name = "unicode-xid" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" - -[[package]] -name = "uuid" -version = "1.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5e395fcf16a7a3d8127ec99782007af141946b4795001f876d54fb0d55978560" -dependencies = [ - "getrandom", - "rand", - "uuid-macro-internal", -] - -[[package]] -name = "uuid-macro-internal" -version = "1.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f49e7f3f3db8040a100710a11932239fd30697115e2ba4107080d8252939845e" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.48", -] - -[[package]] -name = "varisat" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ebe609851d1e9196674ac295f656bd8601200a1077343d22b345013497807caf" -dependencies = [ - "anyhow", - "itoa", - "leb128", - "log", - "ordered-float 2.10.1", - "partial_ref", - "rustc-hash", - "serde", - "thiserror", - "varisat-checker", - "varisat-dimacs", - "varisat-formula", - "varisat-internal-macros", - "varisat-internal-proof", - "vec_mut_scan", -] - -[[package]] -name = "varisat-checker" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "135c977c5913ed6e98f6b81b8e4d322211303b7d40dae773caef7ad1de6c763b" -dependencies = [ - "anyhow", - "log", - "partial_ref", - "rustc-hash", - "smallvec", - "thiserror", - "varisat-dimacs", - "varisat-formula", - "varisat-internal-proof", -] - -[[package]] -name = "varisat-dimacs" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1dee4e21be1f04c0a939f7ae710cced47233a578de08a1b3c7d50848402636" -dependencies = [ - "anyhow", - "itoa", - "thiserror", - "varisat-formula", -] - -[[package]] -name = "varisat-formula" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "395c5543b9bfd9076d6d3af49d6c34a4b91b0b355998c0a5ec6ed7265d364520" - -[[package]] -name = "varisat-internal-macros" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "602ece773543d066aa7848455486c6c0422a3f214da7a2b899100f3c4f12408d" -dependencies = [ - "proc-macro2", - "quote", - "regex", - "syn 1.0.109", - "synstructure", -] - -[[package]] -name = "varisat-internal-proof" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6163bb7bc9018af077b76d64f976803d141c36a27d640f1437dddc4fd527d207" -dependencies = [ - "anyhow", - "varisat-formula", -] - -[[package]] -name = "vec_mut_scan" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "68ed610a8d5e63d9c0e31300e8fdb55104c5f21e422743a9dc74848fa8317fd2" - -[[package]] -name = "wasi" -version = "0.11.0+wasi-snapshot-preview1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" - -[[package]] -name = "wasm-bindgen" -version = "0.2.89" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ed0d4f68a3015cc185aff4db9506a015f4b96f95303897bfa23f846db54064e" -dependencies = [ - "cfg-if", - "wasm-bindgen-macro", -] - -[[package]] -name = "wasm-bindgen-backend" -version = "0.2.89" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b56f625e64f3a1084ded111c4d5f477df9f8c92df113852fa5a374dbda78826" -dependencies = [ - "bumpalo", - "log", - "once_cell", - "proc-macro2", - "quote", - "syn 2.0.48", - "wasm-bindgen-shared", -] - -[[package]] -name = "wasm-bindgen-macro" -version = "0.2.89" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0162dbf37223cd2afce98f3d0785506dcb8d266223983e4b5b525859e6e182b2" -dependencies = [ - "quote", - "wasm-bindgen-macro-support", -] - -[[package]] -name = "wasm-bindgen-macro-support" -version = "0.2.89" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0eb82fcb7930ae6219a7ecfd55b217f5f0893484b7a13022ebb2b2bf20b5283" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.48", - "wasm-bindgen-backend", - "wasm-bindgen-shared", -] - -[[package]] -name = "wasm-bindgen-shared" -version = "0.2.89" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ab9b36309365056cd639da3134bf87fa8f3d86008abf99e612384a6eecd459f" - -[[package]] -name = "which" -version = "4.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" -dependencies = [ - "either", - "home", - "once_cell", - "rustix", -] - -[[package]] -name = "windows-core" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "33ab640c8d7e35bf8ba19b884ba838ceb4fba93a4e8c65a9059d08afcfc683d9" -dependencies = [ - "windows-targets 0.52.0", -] - -[[package]] -name = "windows-sys" -version = "0.48.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" -dependencies = [ - "windows-targets 0.48.5", -] - -[[package]] -name = "windows-sys" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" -dependencies = [ - "windows-targets 0.52.0", -] - -[[package]] -name = "windows-targets" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" -dependencies = [ - "windows_aarch64_gnullvm 0.48.5", - "windows_aarch64_msvc 0.48.5", - "windows_i686_gnu 0.48.5", - "windows_i686_msvc 0.48.5", - "windows_x86_64_gnu 0.48.5", - "windows_x86_64_gnullvm 0.48.5", - "windows_x86_64_msvc 0.48.5", -] - -[[package]] -name = "windows-targets" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8a18201040b24831fbb9e4eb208f8892e1f50a37feb53cc7ff887feb8f50e7cd" -dependencies = [ - "windows_aarch64_gnullvm 0.52.0", - "windows_aarch64_msvc 0.52.0", - "windows_i686_gnu 0.52.0", - "windows_i686_msvc 0.52.0", - "windows_x86_64_gnu 0.52.0", - "windows_x86_64_gnullvm 0.52.0", - "windows_x86_64_msvc 0.52.0", -] - -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" - -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb7764e35d4db8a7921e09562a0304bf2f93e0a51bfccee0bd0bb0b666b015ea" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bbaa0368d4f1d2aaefc55b6fcfee13f41544ddf36801e793edbbfd7d7df075ef" - -[[package]] -name = "windows_i686_gnu" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" - -[[package]] -name = "windows_i686_gnu" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a28637cb1fa3560a16915793afb20081aba2c92ee8af57b4d5f28e4b3e7df313" - -[[package]] -name = "windows_i686_msvc" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" - -[[package]] -name = "windows_i686_msvc" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ffe5e8e31046ce6230cc7215707b816e339ff4d4d67c65dffa206fd0f7aa7b9a" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d6fa32db2bc4a2f5abeacf2b69f7992cd09dca97498da74a151a3132c26befd" - -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" - -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1a657e1e9d3f514745a572a6846d3c7aa7dbe1658c056ed9c3344c4109a6949e" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.48.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dff9641d1cd4be8d1a070daf9e3773c5f67e78b4d9d42263020c057706765c04" - -[[patch.unused]] -name = "action_msgs" -version = "1.2.1" - -[[patch.unused]] -name = "actionlib_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "composition_interfaces" -version = "1.2.1" - -[[patch.unused]] -name = "diagnostic_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "example_interfaces" -version = "0.9.3" - -[[patch.unused]] -name = "examples_rclrs_message_demo" -version = "0.4.1" - -[[patch.unused]] -name = "examples_rclrs_minimal_client_service" -version = "0.4.1" - -[[patch.unused]] -name = "examples_rclrs_minimal_pub_sub" -version = "0.4.1" - -[[patch.unused]] -name = "geometry_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "lifecycle_msgs" -version = "1.2.1" - -[[patch.unused]] -name = "nav_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "rcl_interfaces" -version = "1.2.1" - -[[patch.unused]] -name = "rclrs_example_msgs" -version = "0.4.1" - -[[patch.unused]] -name = "rclrs_tests" -version = "0.4.1" - -[[patch.unused]] -name = "rosgraph_msgs" -version = "1.2.1" - -[[patch.unused]] -name = "sensor_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "shape_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "statistics_msgs" -version = "1.2.1" - -[[patch.unused]] -name = "std_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "std_srvs" -version = "4.2.3" - -[[patch.unused]] -name = "stereo_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "test_msgs" -version = "1.2.1" - -[[patch.unused]] -name = "trajectory_msgs" -version = "4.2.3" - -[[patch.unused]] -name = "unique_identifier_msgs" -version = "2.2.1" - -[[patch.unused]] -name = "visualization_msgs" -version = "4.2.3" diff --git a/rmf_reservation_ros2/Cargo.toml b/rmf_reservation_ros2/Cargo.toml deleted file mode 100644 index 191ecd784..000000000 --- a/rmf_reservation_ros2/Cargo.toml +++ /dev/null @@ -1,27 +0,0 @@ -[package] -name = "rmf_reservation_ros2" -version = "0.3.1" -authors = ["Arjo Chakravaty "] -edition = "2021" - -[[bin]] -name = "reservation_node" -path = "src/reservation_node.rs" - -[dependencies] -anyhow = {version = "1", features = ["backtrace"]} -rmf_reservations = {git = "https://github.com/open-rmf/rmf_reservation", rev = "41f01f8"} -# TODO(arjo): Remove chrono as un upstream dependency -chrono = "*" - -[dependencies.rclrs] -version = "0.4.1" - -[dependencies.rosidl_runtime_rs] -version = "0.4.1" - -[dependencies.rmf_chope_msgs] -version = "*" - -[dependencies.builtin_interfaces] -version = "*" diff --git a/rmf_reservation_ros2/package.xml b/rmf_reservation_ros2/package.xml deleted file mode 100644 index 3fd2fee8b..000000000 --- a/rmf_reservation_ros2/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - rmf_reservation_ros2 - 0.3.1 - Package containing an example of the publish-subscribe mechanism in rclrs. - Arjo Chakravarty - Apache License 2.0 - - rclrs - rosidl_runtime_rs - std_msgs - - rclrs - rosidl_runtime_rs - rmf_chope_msgs - builtin_interfaces - - - ament_cargo - - \ No newline at end of file diff --git a/rmf_reservation_ros2/src/reservation_node.rs b/rmf_reservation_ros2/src/reservation_node.rs deleted file mode 100644 index cb1d1a03d..000000000 --- a/rmf_reservation_ros2/src/reservation_node.rs +++ /dev/null @@ -1,172 +0,0 @@ -use std::env; - -use chrono::Utc; -use anyhow::{Error, Result}; -use rmf_chope_msgs::msg::{FlexibleTimeRequest, DelayRequest, Ticket as RmfTicket, ClaimRequest, ReservationAllocation}; -use rmf_reservations::{database::{FlexibleTimeReservationSystem, Ticket, ClockSource, }, ReservationParameters, ReservationRequest, cost_function::static_cost::StaticCost, StartTimeRange, }; -use rmf_reservations::database::ClaimSpot; - -use std::sync::{Arc, Mutex}; -use std::collections::HashMap; - -struct ReservationSystem { - system: FlexibleTimeReservationSystem, -} - -impl ReservationSystem { - pub fn create_with_clock(clock_source: RosClockSourceProvider) -> Self { - Self { - system: FlexibleTimeReservationSystem::create_with_clock(clock_source) - } - } -} - -struct RosClockSourceProvider { - node: Arc -} - -impl ClockSource for RosClockSourceProvider { - fn now(&self) -> chrono::DateTime { - return from_ros_time(&to_ros_msg(&self.node.get_clock().now())); - } -} - -fn to_ros_msg(time: &rclrs::Time) -> builtin_interfaces::msg::Time { - let nanosec = time.nsec % 1_000_000_000; - let sec = time.nsec / 1_000_000_000; - - builtin_interfaces::msg::Time { - nanosec: nanosec.try_into().unwrap(), - sec: sec.try_into().unwrap() - } -} - -fn to_ros_time(time: &chrono::DateTime) -> builtin_interfaces::msg::Time { - builtin_interfaces::msg::Time { - nanosec: (time.timestamp_nanos() - time.timestamp() * 1_000_000_000).try_into().unwrap(), - sec: time.timestamp().try_into().unwrap() - } -} - -fn from_ros_time(time: &builtin_interfaces::msg::Time) -> chrono::DateTime { - chrono::DateTime::::from_timestamp(time.sec.into(), time.nanosec.into()).unwrap() -} - -fn from_ros_duration(duration: &builtin_interfaces::msg::Duration) -> chrono::Duration { - chrono::Duration::nanoseconds(duration.nanosec.into()) + chrono::Duration::seconds(duration.sec.into()) -} - -fn main() -> Result<(), Error> { - let context = rclrs::Context::new(env::args())?; - - let node = rclrs::create_node(&context, "rmf_reservation_service")?; - - let clock_source= RosClockSourceProvider { - node: node.clone() - }; - let reservation_system = Arc::new(Mutex::new(ReservationSystem::create_with_clock(clock_source))); - - let res_sys_handle = reservation_system.clone(); - let publisher = - node.create_publisher::("rmf/reservations/tickets", rclrs::QOS_PROFILE_DEFAULT)?; - - let _request_sub = node.create_subscription::( - "rmf/reservations/request", - rclrs::QOS_PROFILE_DEFAULT, - move |msg: FlexibleTimeRequest| { - let alternative = msg.alternatives.iter().map(|alt| { - ReservationRequest { - parameters: ReservationParameters { - resource_name: alt.resource_name.clone(), - duration: if alt.has_end { - Some(from_ros_duration(&alt.duration)) - } else { - None - }, - start_time: StartTimeRange { - earliest_start: if alt.start_time.has_earliest_start_time { - Some(from_ros_time(&alt.start_time.earliest_start_time)) - } else { - None - }, - latest_start: if alt.start_time.has_latest_start_time { - Some(from_ros_time(&alt.start_time.latest_start_time)) - } else { - None - } - }, - }, - cost_function: Arc::new(StaticCost::new(alt.cost)) - } - }); - - println!("Recieved request."); - - //println!("I heard: '{}'", msg.data); - let mut res_sys = res_sys_handle.lock().unwrap(); - let Ok(ticket) = res_sys.system.request_resources(alternative.collect()) else { - println!("Could not claim ticket. It was likely already claimed"); - return; - }; - //res_sys.mapping.insert(req_header, ticket.clone()); - let ticket = RmfTicket { - header: msg.header, - ticket_id: ticket.get_id() as u64 - }; - publisher.publish(ticket); - }, - )?; - - let res_sys_handle = reservation_system.clone(); - let publisher = - node.create_publisher::("rmf/reservations/allocation", rclrs::QOS_PROFILE_DEFAULT)?; - let _claim_sub = node.create_subscription::( - "rmf/reservations/claim", - rclrs::QOS_PROFILE_DEFAULT, - move |msg: ClaimRequest| { - let mut res_sys = res_sys_handle.lock().unwrap(); - let resp = res_sys.system.claim_request(Ticket::from_id(msg.ticket.ticket_id as usize), &msg.wait_points); - let Ok(result) = resp else { - println!("Got error message while trying to claim: {}", resp.unwrap_err()); - return; - }; - - let allocation = match result { - ClaimSpot::GoImmediately(goal) => { - ReservationAllocation { - ticket: msg.ticket.clone(), - instruction_type: ReservationAllocation::IMMEDIATELY_PROCEED, - satisfies_alternative: goal.satisfies_alt as u64, - resource: goal.resource.clone(), - time_to_reach: to_ros_time(&goal.time), - alternate_waitpoint: Default::default() - } - }, - ClaimSpot::WaitAtThenGo(id, goal) => { - ReservationAllocation { - ticket: msg.ticket.clone(), - instruction_type: ReservationAllocation::WAIT_AT_SPOT_AND_THEN_GO, - satisfies_alternative: goal.satisfies_alt as u64, - resource: goal.resource.clone(), - time_to_reach: to_ros_time(&goal.time), - alternate_waitpoint: msg.wait_points[id].clone() - } - }, - ClaimSpot::WaitPermanently(id) => { - ReservationAllocation { - ticket: msg.ticket.clone(), - instruction_type: ReservationAllocation::WAIT_PERMANENTLY, - satisfies_alternative: Default::default(), - resource: Default::default(), - time_to_reach: Default::default(), - alternate_waitpoint: msg.wait_points[id].clone() - } - } - }; - - publisher.publish(allocation); - } - ); - - rclrs::spin(node).map_err(|err| err.into()) -} From 76100b62e0554e75c43c345943614e84e4005880 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 5 Feb 2024 11:20:23 +0800 Subject: [PATCH 018/119] Adds a simple parking spot management system. This commit adds a stupidly simple parking spot system. It ensures the next location a robot goes to is not occupied. It also publishes currently free parking spots. When combined with #308 it allows a robot to move to the nearest free spot instead of the nearest spot only. note: This PR still needs more testing Depends on: - open-rmf/rmf_internal_msgs#63 Known issues: - When the robots start in simulation not all robots seem to be running the idle task, hence some robot dont end up claiming parking spots. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/CMakeLists.txt | 4 +- rmf_chope_node/package.xml | 2 +- rmf_chope_node/src/main.cpp | 75 ++++++++++++++++--- .../rmf_fleet_adapter/StandardNames.hpp | 1 + .../src/rmf_fleet_adapter/agv/Node.cpp | 9 +++ .../src/rmf_fleet_adapter/agv/Node.hpp | 9 ++- .../agv/ReservationManager.hpp | 8 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 5 ++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 5 ++ .../rmf_fleet_adapter/events/GoToPlace.cpp | 3 +- 10 files changed, 103 insertions(+), 18 deletions(-) diff --git a/rmf_chope_node/CMakeLists.txt b/rmf_chope_node/CMakeLists.txt index 14abc0ad0..526b964fa 100644 --- a/rmf_chope_node/CMakeLists.txt +++ b/rmf_chope_node/CMakeLists.txt @@ -9,13 +9,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmf_chope_msgs REQUIRED) -find_package(rmf_fleet_adapter REQUIRED) +find_package(rmf_building_map_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) add_executable(queue_manager src/main.cpp) -ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs) +ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs rmf_building_map_msgs) install(TARGETS queue_manager diff --git a/rmf_chope_node/package.xml b/rmf_chope_node/package.xml index f7589fc52..e76c50306 100644 --- a/rmf_chope_node/package.xml +++ b/rmf_chope_node/package.xml @@ -12,8 +12,8 @@ ament_lint_auto ament_lint_common + rmf_building_map_msgs rmf_chope_msgs - rmf_fleet_adapter ament_cmake diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 52deaf975..8f956cd49 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -3,17 +3,17 @@ #include "rclcpp/rclcpp.hpp" #include #include -#include +#include #include -#include #include -#include -#include -#include + +#include +#include #include #include #include #include +#include #include #include @@ -122,13 +122,13 @@ struct LocationReq { } }; - /// Implements a simple Mutex. Only one robot can claim a location at a time. /// The current implementation is relatively simplistic and basically checks if a location is occupied or not. /// A queuing system is in the works. Note: It is possible for the current system to get deadlocked. class CurrentState { public: std::vector free_locations() { + std::lock_guard lock(_mtx); std::vector locations; for (auto &[loc, state] : _current_location_reservations) { if (!state.ticket.has_value()) { @@ -137,20 +137,35 @@ class CurrentState { } return locations; } + + void add_location(std::string location) { + std::lock_guard lock(_mtx); + if (_current_location_reservations.count(location) == 0) + { + _current_location_reservations.emplace(location, LocationState {std::nullopt}); + } + } std::optional allocate_lowest_cost_free_spot(const std::vector& incoming_requests, const std::size_t ticket_id) { + if (_ticket_to_location.count(ticket_id) != 0) + { + // Ticket has been allocated. Probably some DDS-ism causing the issue + return std::nullopt; + } + auto requests = incoming_requests; std::sort(requests.begin(), requests.end()); + std::lock_guard lock(_mtx); for (std::size_t i = 0; i < requests.size(); i++) { auto parking = _current_location_reservations.find(requests[i].location); if (parking == _current_location_reservations.end()) { - _current_location_reservations.emplace(requests[i].location, LocationState {ticket_id}); + _current_location_reservations[requests[i].location] = LocationState {ticket_id}; _ticket_to_location.emplace(ticket_id, requests[i].location); return requests[i].location; } - else if (!parking->second.ticket.has_value()){ - _current_location_reservations.emplace(parking->first, LocationState {ticket_id}); + else if (!parking->second.ticket.has_value()) { + _current_location_reservations[requests[i].location].ticket = ticket_id; _ticket_to_location.emplace(ticket_id, requests[i].location); return parking->first; } @@ -161,6 +176,7 @@ class CurrentState { bool release(const std::size_t ticket_id) { + std::lock_guard lock(_mtx); auto _ticket = _ticket_to_location.find(ticket_id); if (_ticket == _ticket_to_location.end()) { @@ -172,10 +188,13 @@ class CurrentState { } private: + std::mutex _mtx; std::unordered_map _current_location_reservations; std::unordered_map _ticket_to_location; }; +using namespace std::chrono_literals; + class SimpleQueueSystemNode : public rclcpp::Node { public: SimpleQueueSystemNode() : Node("rmf_chope_node") { @@ -191,12 +210,34 @@ class SimpleQueueSystemNode : public rclcpp::Node { ReservationClaimTopicName, qos, std::bind(&SimpleQueueSystemNode::claim_request, this, std::placeholders::_1)); release_subscription_ = this->create_subscription( ReservationReleaseTopicName, qos, std::bind(&SimpleQueueSystemNode::release, this, std::placeholders::_1)); + graph_subscription_ = this->create_subscription("/nav_graphs", qos, std::bind(&SimpleQueueSystemNode::recieved_graph, this, std::placeholders::_1)); ticket_pub_ = this->create_publisher(ReservationResponseTopicName, qos); allocation_pub_ = this->create_publisher(ReservationAllocationTopicName, qos); + free_spot_pub_ = this->create_publisher("/rmf/reservations/free_parking_spot", qos); + + timer_ = this->create_wall_timer(500ms, std::bind(&SimpleQueueSystemNode::publish_free_spots, this)); } private: + void recieved_graph(const rmf_building_map_msgs::msg::Graph::ConstSharedPtr &graph_msg) { + RCLCPP_INFO(this->get_logger(), "Got graph"); + for (std::size_t i = 0; i < graph_msg->vertices.size(); i++) + { + for(auto ¶m: graph_msg->vertices[i].params) { + + //TODO(arjo) make this configure-able + if (param.name == "is_parking_spot" && param.value_bool) + { + std::stringstream name; + name << i; + std::string topic; + name >> topic; + current_state_.add_location(topic); + } + } + } + } void on_request(const rmf_chope_msgs::msg::FlexibleTimeRequest::ConstSharedPtr &request) { std::vector requests; @@ -249,12 +290,12 @@ class SimpleQueueSystemNode : public rclcpp::Node { allocation.satisfies_alternative = i; } } - + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", result.value().c_str(), request->ticket.ticket_id); allocation_pub_->publish(allocation); } else { - RCLCPP_ERROR(this->get_logger(), "Could not allocate resource for ticket %lu.", request->ticket.ticket_id); + RCLCPP_INFO(this->get_logger(), "Could not allocate resource for ticket %lu.", request->ticket.ticket_id); } } @@ -267,16 +308,28 @@ class SimpleQueueSystemNode : public rclcpp::Node { } } + + void publish_free_spots() { + rmf_chope_msgs::msg::FreeParkingSpots spots; + spots.spots = current_state_.free_locations(); + + free_spot_pub_->publish(spots); + } + rclcpp::Subscription::SharedPtr request_subscription_; rclcpp::Subscription::SharedPtr claim_subscription_; rclcpp::Subscription::SharedPtr release_subscription_; + rclcpp::Subscription::SharedPtr graph_subscription_; rclcpp::Publisher::SharedPtr ticket_pub_; rclcpp::Publisher::SharedPtr allocation_pub_; + rclcpp::Publisher::SharedPtr free_spot_pub_; std::unordered_map> requests_; TicketStore ticket_store_; CurrentState current_state_; + + rclcpp::TimerBase::SharedPtr timer_; }; int main (int argc, const char** argv) { diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 6cd190f60..eb9f62759 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -78,6 +78,7 @@ const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; +const std::string ReservationFreeSpotsTopicName = "/rmf/reservations/free_parking_spot"; const uint64_t Unclaimed = (uint64_t)(-1); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index d72acf912..187be1d46 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -139,6 +139,10 @@ std::shared_ptr Node::make( node->_reservation_release_pub = node->create_publisher( ReservationReleaseTopicName, transient_local_qos); + + node->_reservation_free_spot_obs = + node->create_observable( + ReservationFreeSpotsTopicName, transient_local_qos); return node; } @@ -310,5 +314,10 @@ auto Node::release_location() const -> const ReservationReleasePub& return _reservation_release_pub; } +//============================================================================== +auto Node::freespots_obs() const -> const ReservationFreeSpotObs& +{ + return _reservation_free_spot_obs->observe(); +} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 88fca8033..4f25bee9d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -18,8 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP #define SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP -#include -#include + #include #include @@ -39,6 +38,7 @@ #include #include #include +#include #include @@ -163,6 +163,10 @@ class Node : public rmf_rxcpp::Transport using ReservationReleasePub = rclcpp::Publisher::SharedPtr; const ReservationReleasePub& release_location() const; + using ReservationFreeSpotStatus = rmf_chope_msgs::msg::FreeParkingSpots; + using ReservationFreeSpotObs = rxcpp::observable; + const ReservationFreeSpotObs& freespots_obs() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( std::chrono::duration period, @@ -228,6 +232,7 @@ class Node : public rmf_rxcpp::Transport ReservationClaimPub _reservation_claim_pub; Bridge _reservation_alloc_obs; ReservationReleasePub _reservation_release_pub; + Bridge _reservation_free_spot_obs; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index 2f4a369ac..1e70f78c4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -38,7 +38,8 @@ class ReservationManager { } std::optional release_ticket() { - if (allocations.size() == 0) { + if (allocations.size() <= 1) { + // For safety every robot must have at least one reservation at any point in time. return std::nullopt; } auto temp = allocations.back(); @@ -46,6 +47,11 @@ class ReservationManager { return temp; } + bool has_ticket() const + { + return allocations.size() != 0; + } + std::deque allocations; }; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index bc27f51ea..de088aac1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1614,5 +1614,10 @@ std::optional return _reservation_mgr.release_ticket(); } +//============================================================================== +bool RobotContext::_has_ticket() const +{ + return _reservation_mgr.has_ticket(); +} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 335386fd0..c81cf88c0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -720,6 +720,9 @@ class RobotContext std::optional _release_resource(); + /// Has ticket now + bool _has_ticket() const; + template static std::shared_ptr make(Args&&... args) { @@ -770,6 +773,8 @@ class RobotContext self->_publish_mutex_group_requests(); }); + //context-> + return context; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 8232a4682..c04649531 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -521,7 +521,8 @@ std::optional GoToPlace::Active::_choose_goal( // No need to use reservation system if we are already there. if (_description.one_of().size() == 1 - && _description.one_of()[0].waypoint() == current_location[0].waypoint()) + && _description.one_of()[0].waypoint() == current_location[0].waypoint() + && _context->_has_ticket()) { return _description.one_of()[0]; } From 705028a8aea05745b02b2811dcb2a01fef85b593 Mon Sep 17 00:00:00 2001 From: Grey Date: Thu, 21 Dec 2023 21:57:43 +0800 Subject: [PATCH 019/119] Fix edge case when starting on a lane (#312) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index bc9db015c..780deab8f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1052,6 +1052,27 @@ void EasyCommandHandle::follow_new_path( i0 = i; } } + + if (l.lane().has_value()) + { + Eigen::Vector2d p_l; + if (l.location().has_value()) + { + p_l = *l.location(); + } + else + { + p_l = graph.get_waypoint(l.waypoint()).get_location(); + } + const double dist = (wp.position().block<2, 1>(0, 0) - p_l).norm(); + const double merge_dist = graph.get_waypoint(*wp.graph_index()) + .merge_radius().value_or(nav_params->max_merge_lane_distance); + if (dist <= merge_dist) + { + found_connection = true; + i0 = i; + } + } } } else @@ -1099,10 +1120,21 @@ void EasyCommandHandle::follow_new_path( { // The robot has drifted away from the starting point since the plan started // so we'll ask for a new plan. + std::stringstream ss; + ss << "Current location" << print_starts(current_location, graph); + ss << "\nCommanded path:"; + const auto t0 = waypoints.front().time(); + for (const auto& wp : waypoints) + { + ss << "\n -- " << print_plan_waypoint(wp, graph, t0); + } + RCLCPP_INFO( context->node()->get_logger(), - "Requesting replan for [%s] because it is too far from its commanded path", - context->requester_id().c_str()); + "Requesting replan for [%s] because it is too far from its commanded " + "path. Details:\n%s", + context->requester_id().c_str(), + ss.str().c_str()); // Stop the robot to prevent it from diverging too far while a replan // happens. From fe3534c46135cb3d279283a866b460444394fcb7 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 22 Dec 2023 16:07:22 +0800 Subject: [PATCH 020/119] Update changelogs and 2.5.0 (#313) Signed-off-by: Yadunund Signed-off-by: Arjo Chakravarty --- rmf_charging_schedule/CHANGELOG.rst | 3 +++ rmf_charging_schedule/package.xml | 2 +- rmf_charging_schedule/setup.py | 2 +- rmf_fleet_adapter/CHANGELOG.rst | 6 ++++++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 3 +++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 3 +++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 3 +++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 3 +++ rmf_websocket/package.xml | 2 +- 13 files changed, 28 insertions(+), 7 deletions(-) diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst index 2955530ac..9215a03c4 100644 --- a/rmf_charging_schedule/CHANGELOG.rst +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_charging_schedule ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.0 (2023-12-22) +------------------ + 2.4.0 (2023-12-15) ------------------ * First release diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index 70ef0b94a..84d034d8f 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -2,7 +2,7 @@ rmf_charging_schedule - 2.4.0 + 2.5.0 Node for a fixed 24-hour rotating charger usage schedule Grey Apache License 2.0 diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py index 6ae87a05e..55754a0d6 100644 --- a/rmf_charging_schedule/setup.py +++ b/rmf_charging_schedule/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version='2.3.2', + version='2.5.0', packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index 8da420f87..0062e4071 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.0 (2023-12-22) +------------------ +* Fix edge case when starting on a lane (`#312 `_) +* Update `GoToPlace` to allow finding nearest spot (`#308 `_) +* Contributors: Arjo Chakravarty, Grey + 2.4.0 (2023-12-15) ------------------ * Mutex Groups, localization hook, dynamic charging, and new graph elements (`#310 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 82a680b6e..ffbcdc5a6 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.4.0 + 2.5.0 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index 6c7392978..9191dcdf4 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.0 (2023-12-22) +------------------ + 2.4.0 (2023-12-15) ------------------ diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index 340238924..16a4e4dea 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.4.0 + 2.5.0 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index 03d119f50..250fac739 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.0 (2023-12-22) +------------------ + 2.4.0 (2023-12-15) ------------------ diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 3f99ff959..fcc39c053 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.4.0 + 2.5.0 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index 042b5f4e2..c3169e89f 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.0 (2023-12-22) +------------------ + 2.4.0 (2023-12-15) ------------------ diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 812c3988b..eff1187b5 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.4.0 + 2.5.0 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index 871ac8afe..8ef63be1a 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.5.0 (2023-12-22) +------------------ + 2.4.0 (2023-12-15) ------------------ diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index 7da49a00d..a884dac6a 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.4.0 + 2.5.0 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From 91ce68b1a23b8c5e7718f4fa63e98ff7cd9795a1 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Jan 2024 21:51:04 +0800 Subject: [PATCH 021/119] Adds an option to generate unique hex strings for new task ids, fix time stamp logic (#319) Signed-off-by: Aaron Chong Signed-off-by: Arjo Chakravarty --- .../src/rmf_task_ros2/Dispatcher.cpp | 43 ++++++++++++++++++- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 07c973927..f6801f375 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -46,6 +46,7 @@ #include #include +#include #include namespace rmf_task_ros2 { @@ -90,6 +91,24 @@ nlohmann::json_schema::json_validator make_validator(nlohmann::json schema) return nlohmann::json_schema::json_validator( std::move(schema), schema_loader); } + +//============================================================================== +std::string generate_random_hex_string(const std::size_t length = 3) +{ + std::stringstream ss; + for (std::size_t i = 0; i < length; ++i) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(0, 255); + const auto random_char = dis(gen); + std::stringstream hexstream; + hexstream << std::hex << random_char; + auto hex = hexstream.str(); + ss << (hex.length() < 2 ? '0' + hex : hex); + } + return ss.str(); +} } // anonymous namespace //============================================================================== @@ -176,6 +195,7 @@ class Dispatcher::Implementation std::size_t terminated_tasks_max_size; int publish_active_tasks_period; bool use_timestamp_for_task_id; + bool use_unique_hex_string_with_task_id; std::unordered_map legacy_task_type_names = { @@ -215,6 +235,12 @@ class Dispatcher::Implementation RCLCPP_INFO(node->get_logger(), " Use timestamp with task_id: %s", (use_timestamp_for_task_id ? "true" : "false")); + use_unique_hex_string_with_task_id = + node->declare_parameter("use_unique_hex_string_with_task_id", + false); + RCLCPP_INFO(node->get_logger(), + " Use unique hex string with task_id: %s", + (use_unique_hex_string_with_task_id ? "true" : "false")); std::optional server_uri = std::nullopt; const std::string uri = @@ -483,10 +509,23 @@ class Dispatcher::Implementation task_request_json["category"].get() + ".dispatch-"; - if (use_timestamp_for_task_id) + if (use_unique_hex_string_with_task_id) + { + if (use_timestamp_for_task_id) + { + RCLCPP_WARN( + node->get_logger(), + "Overwriting use_timestamp_for_task_id option with " + "use_unique_hex_string_with_task_id" + ); + } + task_id += generate_random_hex_string(5); + } + else if (use_timestamp_for_task_id) { task_id += std::to_string( - static_cast(node->get_clock()->now().nanoseconds()/1e6)); + static_cast(std::round(node->get_clock()->now().seconds() * + 1e3))); } else { From 7c5189858f2d6d248faa461e37481c0471fe5a08 Mon Sep 17 00:00:00 2001 From: cwrx777 Date: Fri, 26 Jan 2024 15:45:26 +0800 Subject: [PATCH 022/119] Add support of fleet-level task (#317) * check if task request has fleet_name and reject if it does not match fleet adapter's name. Signed-off-by: Charly Wu * update logging. Signed-off-by: Charly Wu * added DCO commit Signed-off-by: Charly Wu * delete unnecessary new lines. Signed-off-by: Charly Wu * remove extra space. Signed-off-by: Charly Wu * uncrustified. Signed-off-by: Charly Wu --------- Signed-off-by: Charly Wu Co-authored-by: Yadu Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 898b381af..14481a579 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -427,7 +427,6 @@ class AllocateTasks : public std::enable_shared_from_this std::shared_ptr node; }; - //============================================================================== void FleetUpdateHandle::Implementation::bid_notice_cb( const BidNoticeMsg& bid_notice, @@ -493,6 +492,18 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( }); } + // If a fleet_name was specified in the request, only proceed if the value matches + // the name of this fleet. + if (request_msg.contains("fleet_name") && + request_msg["fleet_name"].template get() != name) + { + RCLCPP_INFO( + node->get_logger(), + "Ignoring BidNotice request as it is for fleet [%s].", + request_msg["fleet_name"].template get().c_str()); + return; + } + std::vector errors = {}; const auto new_request = convert(task_id, request_msg, errors); if (!new_request) From ed96418bdb5700a3f56b6b67a0a16da002442226 Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Wed, 31 Jan 2024 14:00:27 +0800 Subject: [PATCH 023/119] use keep_last(10) for all instances without a depth already set Signed-off-by: Teo Koon Peng Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/close_lanes/main.cpp | 4 ++-- rmf_fleet_adapter/src/door_supervisor/Node.cpp | 2 +- .../src/experimental_lift_watchdog/main.cpp | 2 +- rmf_fleet_adapter/src/full_control/main.cpp | 14 +++++++------- rmf_fleet_adapter/src/interrupt_robot/main.cpp | 2 +- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 4 ++-- rmf_fleet_adapter/src/mock_traffic_light/main.cpp | 10 +++++----- .../src/mutex_group_supervisor/main.cpp | 2 +- rmf_fleet_adapter/src/open_lanes/main.cpp | 4 ++-- .../src/read_only/FleetAdapterNode.cpp | 2 +- .../src/read_only_blockade/FleetAdapterNode.cpp | 2 +- .../src/rmf_fleet_adapter/agv/Node.cpp | 6 +++--- .../RobotStateAggregator.cpp | 2 +- rmf_fleet_adapter/test/dump_fleet_states.cpp | 2 +- rmf_fleet_adapter/test/tasks/test_Delivery.cpp | 8 ++++---- rmf_fleet_adapter/test/test_read_only_adapter.cpp | 6 +++--- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 4 ++-- .../src/rmf_traffic_ros2/blockade/Node.cpp | 12 ++++++------ .../src/rmf_traffic_ros2/blockade/Writer.cpp | 12 ++++++------ .../rmf_traffic_ros2/schedule/MirrorManager.cpp | 2 +- .../src/rmf_traffic_ros2/schedule/Node.cpp | 10 +++++----- .../src/rmf_traffic_ros2/schedule/Writer.cpp | 6 +++--- 22 files changed, 59 insertions(+), 59 deletions(-) diff --git a/rmf_fleet_adapter/src/close_lanes/main.cpp b/rmf_fleet_adapter/src/close_lanes/main.cpp index 0191f8452..ef00386fc 100644 --- a/rmf_fleet_adapter/src/close_lanes/main.cpp +++ b/rmf_fleet_adapter/src/close_lanes/main.cpp @@ -96,7 +96,7 @@ int main(int argc, char* argv[]) const auto publisher = node->create_publisher( rmf_fleet_adapter::LaneClosureRequestTopicName, - rclcpp::SystemDefaultsQoS().transient_local()); + rclcpp::SystemDefaultsQoS().keep_last(10).transient_local()); publisher->publish(request); @@ -111,7 +111,7 @@ int main(int argc, char* argv[]) const auto listener = node->create_subscription( rmf_fleet_adapter::ClosedLaneTopicName, - rclcpp::SystemDefaultsQoS().transient_local(), + rclcpp::SystemDefaultsQoS().keep_last(10).transient_local(), [&request_complete, fleet_name, close_lanes = std::move(close_lanes)]( std::unique_ptr msg) { diff --git a/rmf_fleet_adapter/src/door_supervisor/Node.cpp b/rmf_fleet_adapter/src/door_supervisor/Node.cpp index f3ed2781f..5b5f149af 100644 --- a/rmf_fleet_adapter/src/door_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/door_supervisor/Node.cpp @@ -28,7 +28,7 @@ const std::string DoorSupervisorRequesterID = "door_supervisor"; Node::Node() : rclcpp::Node("door_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); _door_request_pub = create_publisher( FinalDoorRequestTopicName, default_qos); diff --git a/rmf_fleet_adapter/src/experimental_lift_watchdog/main.cpp b/rmf_fleet_adapter/src/experimental_lift_watchdog/main.cpp index 00c2c8afc..49a39c63a 100644 --- a/rmf_fleet_adapter/src/experimental_lift_watchdog/main.cpp +++ b/rmf_fleet_adapter/src/experimental_lift_watchdog/main.cpp @@ -43,7 +43,7 @@ class ExperimentalLiftWatchdogNode : public rclcpp::Node }); permission_subscription = create_subscription( - "experimental_lift_permission", rclcpp::SystemDefaultsQoS(), + "experimental_lift_permission", rclcpp::SystemDefaultsQoS().keep_last(10), [=](std::shared_ptr msg) { permission_callback(msg); diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 2888482da..f84f3b03b 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -1027,7 +1027,7 @@ std::shared_ptr make_fleet( connections->lane_closure_request_sub = adapter->node()->create_subscription( rmf_fleet_adapter::LaneClosureRequestTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [w = connections->weak_from_this(), fleet_name]( rmf_fleet_msgs::msg::LaneRequest::UniquePtr request_msg) { @@ -1071,7 +1071,7 @@ std::shared_ptr make_fleet( adapter->node()->create_subscription< rmf_fleet_msgs::msg::SpeedLimitRequest>( rmf_fleet_adapter::SpeedLimitRequestTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [w = connections->weak_from_this(), fleet_name]( rmf_fleet_msgs::msg::SpeedLimitRequest::ConstSharedPtr request_msg) { @@ -1099,7 +1099,7 @@ std::shared_ptr make_fleet( connections->interrupt_request_sub = adapter->node()->create_subscription( rmf_fleet_adapter::InterruptRequestTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [w = connections->weak_from_this(), fleet_name]( rmf_fleet_msgs::msg::InterruptRequest::UniquePtr request_msg) { @@ -1128,7 +1128,7 @@ std::shared_ptr make_fleet( connections->mode_request_sub = adapter->node()->create_subscription( "/action_execution_notice", - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [w = connections->weak_from_this(), fleet_name]( rmf_fleet_msgs::msg::ModeRequest::UniquePtr msg) { @@ -1358,16 +1358,16 @@ std::shared_ptr make_fleet( connections->path_request_pub = node->create_publisher< rmf_fleet_msgs::msg::PathRequest>( - rmf_fleet_adapter::PathRequestTopicName, rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::PathRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); connections->mode_request_pub = node->create_publisher< rmf_fleet_msgs::msg::ModeRequest>( - rmf_fleet_adapter::ModeRequestTopicName, rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::ModeRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); connections->fleet_state_sub = node->create_subscription< rmf_fleet_msgs::msg::FleetState>( rmf_fleet_adapter::FleetStateTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [c = std::weak_ptr(connections), fleet_name]( const rmf_fleet_msgs::msg::FleetState::SharedPtr msg) { diff --git a/rmf_fleet_adapter/src/interrupt_robot/main.cpp b/rmf_fleet_adapter/src/interrupt_robot/main.cpp index 61239e803..ccf19ed01 100644 --- a/rmf_fleet_adapter/src/interrupt_robot/main.cpp +++ b/rmf_fleet_adapter/src/interrupt_robot/main.cpp @@ -74,7 +74,7 @@ int main(int argc, char* argv[]) const auto publisher = node->create_publisher( rmf_fleet_adapter::InterruptRequestTopicName, - rclcpp::SystemDefaultsQoS()); + rclcpp::SystemDefaultsQoS().keep_last(10)); publisher->publish(request); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 77c05fdcb..c15346033 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -27,8 +27,8 @@ namespace lift_supervisor { Node::Node() : rclcpp::Node("rmf_lift_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS(); - const auto transient_qos = rclcpp::SystemDefaultsQoS() + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + const auto transient_qos = rclcpp::SystemDefaultsQoS().keep_last(10) .reliable().keep_last(100).transient_local(); _lift_request_pub = create_publisher( diff --git a/rmf_fleet_adapter/src/mock_traffic_light/main.cpp b/rmf_fleet_adapter/src/mock_traffic_light/main.cpp index d410339ec..ba618ca90 100644 --- a/rmf_fleet_adapter/src/mock_traffic_light/main.cpp +++ b/rmf_fleet_adapter/src/mock_traffic_light/main.cpp @@ -797,20 +797,20 @@ std::shared_ptr make_fleet( connections->path_request_pub = node->create_publisher< rmf_fleet_msgs::msg::PathRequest>( - rmf_fleet_adapter::PathRequestTopicName, rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::PathRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); connections->pause_request_pub = node->create_publisher< rmf_fleet_msgs::msg::PauseRequest>( - rmf_fleet_adapter::PauseRequestTopicName, rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::PauseRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); connections->mode_request_pub = node->create_publisher< rmf_fleet_msgs::msg::ModeRequest>( - rmf_fleet_adapter::ModeRequestTopicName, rclcpp::SystemDefaultsQoS()); + rmf_fleet_adapter::ModeRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); connections->fleet_state_sub = node->create_subscription< rmf_fleet_msgs::msg::FleetState>( rmf_fleet_adapter::FleetStateTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [c = std::weak_ptr(connections), fleet_name]( const rmf_fleet_msgs::msg::FleetState::SharedPtr msg) { @@ -850,7 +850,7 @@ std::shared_ptr make_fleet( connections->loop_sub = node->create_subscription< rmf_task_msgs::msg::Loop>( rmf_fleet_adapter::LoopRequestTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [c = std::weak_ptr(connections), fleet_name]( const rmf_task_msgs::msg::Loop::SharedPtr msg) { diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index 4f07df44d..1e8760238 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -47,7 +47,7 @@ class Node : public rclcpp::Node Node() : rclcpp::Node("mutex_group_supervisor") { - const auto qos = rclcpp::SystemDefaultsQoS() + const auto qos = rclcpp::SystemDefaultsQoS().keep_last(10) .reliable() .transient_local() .keep_last(100); diff --git a/rmf_fleet_adapter/src/open_lanes/main.cpp b/rmf_fleet_adapter/src/open_lanes/main.cpp index 5f96bdfc1..771a180cd 100644 --- a/rmf_fleet_adapter/src/open_lanes/main.cpp +++ b/rmf_fleet_adapter/src/open_lanes/main.cpp @@ -96,7 +96,7 @@ int main(int argc, char* argv[]) const auto publisher = node->create_publisher( rmf_fleet_adapter::LaneClosureRequestTopicName, - rclcpp::SystemDefaultsQoS().transient_local()); + rclcpp::SystemDefaultsQoS().keep_last(10).transient_local()); publisher->publish(request); @@ -111,7 +111,7 @@ int main(int argc, char* argv[]) const auto listener = node->create_subscription( rmf_fleet_adapter::ClosedLaneTopicName, - rclcpp::SystemDefaultsQoS().transient_local(), + rclcpp::SystemDefaultsQoS().keep_last(10).transient_local(), [&request_complete, fleet_name, open_lanes = std::move(open_lanes)]( std::unique_ptr msg) { diff --git a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp index fc0e778a3..fa2e921e1 100644 --- a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp +++ b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp @@ -73,7 +73,7 @@ std::shared_ptr FleetAdapterNode::make() // Don't subscribe until everything else is ready node->_fleet_state_subscription = node->create_subscription( - FleetStateTopicName, rclcpp::SystemDefaultsQoS(), + FleetStateTopicName, rclcpp::SystemDefaultsQoS().keep_last(10), [self = node.get()](FleetState::UniquePtr msg) { self->fleet_state_update(std::move(msg)); diff --git a/rmf_fleet_adapter/src/read_only_blockade/FleetAdapterNode.cpp b/rmf_fleet_adapter/src/read_only_blockade/FleetAdapterNode.cpp index 146752973..c12262c79 100644 --- a/rmf_fleet_adapter/src/read_only_blockade/FleetAdapterNode.cpp +++ b/rmf_fleet_adapter/src/read_only_blockade/FleetAdapterNode.cpp @@ -96,7 +96,7 @@ std::shared_ptr FleetAdapterNode::make() node->_fleet_state_subscription = node->create_subscription( - FleetStateTopicName, rclcpp::SystemDefaultsQoS(), + FleetStateTopicName, rclcpp::SystemDefaultsQoS().keep_last(10), [self = node.get()](FleetState::UniquePtr msg) { self->fleet_state_update(std::move(msg)); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 187be1d46..088583428 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -34,9 +34,9 @@ std::shared_ptr Node::make( auto node = std::shared_ptr( new Node(std::move(worker), node_name, options)); - auto default_qos = rclcpp::SystemDefaultsQoS(); + auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); default_qos.keep_last(100); - auto transient_qos = rclcpp::SystemDefaultsQoS() + auto transient_qos = rclcpp::SystemDefaultsQoS().keep_last(10) .reliable().keep_last(100).transient_local(); node->_door_state_obs = @@ -95,7 +95,7 @@ std::shared_ptr Node::make( node->create_publisher( FleetStateTopicName, default_qos); - auto transient_local_qos = rclcpp::SystemDefaultsQoS() + auto transient_local_qos = rclcpp::SystemDefaultsQoS().keep_last(10) .reliable() .transient_local() .keep_last(100); diff --git a/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp b/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp index 9eba8cd63..772ac0df7 100644 --- a/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp +++ b/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp @@ -43,7 +43,7 @@ class RobotStateAggregator : public rclcpp::Node : rclcpp::Node("robot_state_aggregator", options) { RCLCPP_DEBUG(get_logger(), "RobotStateAggregator called"); - const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); const auto state_qos = rclcpp::SystemDefaultsQoS().keep_last(100); #ifdef FAILOVER_MODE diff --git a/rmf_fleet_adapter/test/dump_fleet_states.cpp b/rmf_fleet_adapter/test/dump_fleet_states.cpp index 2fac4f90d..f3ade3db4 100644 --- a/rmf_fleet_adapter/test/dump_fleet_states.cpp +++ b/rmf_fleet_adapter/test/dump_fleet_states.cpp @@ -60,7 +60,7 @@ class DumpFleetStates : public rclcpp::Node : rclcpp::Node("dump_test_fleet_states") { fleet_state_publisher = create_publisher( - "/fleet_states", rclcpp::SystemDefaultsQoS()); + "/fleet_states", rclcpp::SystemDefaultsQoS().keep_last(10)); timer = create_wall_timer( std::chrono::milliseconds(period_ms), [&]() { update(); }); diff --git a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp index be3b71277..878c0cd60 100644 --- a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp +++ b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp @@ -62,7 +62,7 @@ class MockQuietDispenser dispenser->_request_sub = dispenser->_node->create_subscription( rmf_fleet_adapter::DispenserRequestTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [me = dispenser->weak_from_this()](DispenserRequest::SharedPtr msg) { if (const auto self = me.lock()) @@ -72,7 +72,7 @@ class MockQuietDispenser dispenser->_result_pub = dispenser->_node->create_publisher( rmf_fleet_adapter::DispenserResultTopicName, - rclcpp::SystemDefaultsQoS()); + rclcpp::SystemDefaultsQoS().keep_last(10)); return dispenser; } @@ -171,7 +171,7 @@ class MockFlakyIngestor : public std::enable_shared_from_this ingestor->_request_sub = ingestor->_node->create_subscription( rmf_fleet_adapter::IngestorRequestTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [me = ingestor->weak_from_this()](IngestorRequest::SharedPtr msg) { if (const auto self = me.lock()) @@ -180,7 +180,7 @@ class MockFlakyIngestor : public std::enable_shared_from_this ingestor->_state_pub = ingestor->_node->create_publisher( rmf_fleet_adapter::IngestorStateTopicName, - rclcpp::SystemDefaultsQoS()); + rclcpp::SystemDefaultsQoS().keep_last(10)); using namespace std::chrono_literals; ingestor->_timer = ingestor->_node->create_wall_timer( diff --git a/rmf_fleet_adapter/test/test_read_only_adapter.cpp b/rmf_fleet_adapter/test/test_read_only_adapter.cpp index 828df8f0f..3004e975c 100644 --- a/rmf_fleet_adapter/test/test_read_only_adapter.cpp +++ b/rmf_fleet_adapter/test/test_read_only_adapter.cpp @@ -54,16 +54,16 @@ class TestReadOnly : public rclcpp::Node // publisher to send FleetState messages to fleet adapter _fleet_state_pub = create_publisher( rmf_fleet_adapter::FleetStateTopicName, - rclcpp::SystemDefaultsQoS()); + rclcpp::SystemDefaultsQoS().keep_last(10)); // publisher to send String messages to rmf_schedule_visualizer node _viz_pub = create_publisher( - _viz_name + "/debug", rclcpp::SystemDefaultsQoS()); + _viz_name + "/debug", rclcpp::SystemDefaultsQoS().keep_last(10)); // subscription to receive String messages which control execution of // this node _cmd_sub = create_subscription( - "test_adapter_" + _fleet_name+"/cmd", rclcpp::SystemDefaultsQoS(), + "test_adapter_" + _fleet_name+"/cmd", rclcpp::SystemDefaultsQoS().keep_last(10), [&](String::UniquePtr msg) { // receive commands through ros2 msg diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index f6801f375..cfb2d3ecb 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -261,7 +261,7 @@ class Dispatcher::Implementation // names api_request = node->create_subscription( "task_api_requests", - rclcpp::SystemDefaultsQoS().reliable().transient_local(), + rclcpp::SystemDefaultsQoS().keep_last(10).reliable().transient_local(), [this](const ApiRequestMsg::UniquePtr msg) { this->handle_api_request(*msg); @@ -269,7 +269,7 @@ class Dispatcher::Implementation api_response = node->create_publisher( "task_api_responses", - rclcpp::SystemDefaultsQoS().reliable().transient_local()); + rclcpp::SystemDefaultsQoS().keep_last(10).reliable().transient_local()); // TODO(MXG): The smallest resolution this supports is 1 second. That // doesn't seem great. diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Node.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Node.cpp index 3ddb31b74..c465f78ac 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Node.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Node.cpp @@ -48,7 +48,7 @@ class BlockadeNode : public rclcpp::Node blockade_set_sub = create_subscription( BlockadeSetTopicName, - rclcpp::SystemDefaultsQoS().best_effort(), + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort(), [=](const SetMsg::UniquePtr msg) { this->blockade_set(*msg); @@ -57,7 +57,7 @@ class BlockadeNode : public rclcpp::Node blockade_ready_sub = create_subscription( BlockadeReadyTopicName, - rclcpp::SystemDefaultsQoS().best_effort(), + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort(), [=](const ReadyMsg::UniquePtr msg) { this->blockade_ready(*msg); @@ -66,7 +66,7 @@ class BlockadeNode : public rclcpp::Node blockade_reached_sub = create_subscription( BlockadeReachedTopicName, - rclcpp::SystemDefaultsQoS().best_effort(), + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort(), [=](const ReachedMsg::UniquePtr msg) { this->blockade_reached(*msg); @@ -75,7 +75,7 @@ class BlockadeNode : public rclcpp::Node blockade_release_sub = create_subscription( BlockadeReleaseTopicName, - rclcpp::SystemDefaultsQoS().best_effort(), + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort(), [=](const ReleaseMsg::UniquePtr msg) { this->blockade_release(*msg); @@ -84,7 +84,7 @@ class BlockadeNode : public rclcpp::Node blockade_cancel_sub = create_subscription( BlockadeCancelTopicName, - rclcpp::SystemDefaultsQoS().best_effort(), + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort(), [=](const CancelMsg::UniquePtr msg) { this->blockade_cancel(*msg); @@ -92,7 +92,7 @@ class BlockadeNode : public rclcpp::Node heartbeat_pub = create_publisher( BlockadeHeartbeatTopicName, - rclcpp::SystemDefaultsQoS().reliable()); + rclcpp::SystemDefaultsQoS().keep_last(10).reliable()); heartbeat_timer = create_wall_timer( std::chrono::seconds(1), diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Writer.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Writer.cpp index 7f6e2e257..0cd5513b2 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Writer.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/blockade/Writer.cpp @@ -101,7 +101,7 @@ class RectifierFactory { heartbeat_sub = node.create_subscription( BlockadeHeartbeatTopicName, - rclcpp::SystemDefaultsQoS().reliable(), + rclcpp::SystemDefaultsQoS().keep_last(10).reliable(), [&](const HeartbeatMsg::UniquePtr msg) { check_status(*msg); @@ -271,23 +271,23 @@ class Writer::Implementation { set_pub = node.create_publisher( BlockadeSetTopicName, - rclcpp::SystemDefaultsQoS().best_effort()); + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort()); ready_pub = node.create_publisher( BlockadeReadyTopicName, - rclcpp::SystemDefaultsQoS().best_effort()); + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort()); reached_pub = node.create_publisher( BlockadeReachedTopicName, - rclcpp::SystemDefaultsQoS().best_effort()); + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort()); release_pub = node.create_publisher( BlockadeReleaseTopicName, - rclcpp::SystemDefaultsQoS().best_effort()); + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort()); cancel_pub = node.create_publisher( BlockadeCancelTopicName, - rclcpp::SystemDefaultsQoS().best_effort()); + rclcpp::SystemDefaultsQoS().keep_last(10).best_effort()); } using ParticipantId = rmf_traffic::blockade::ParticipantId; diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp index aa294ca25..5841ef313 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp @@ -117,7 +117,7 @@ class MirrorManager::Implementation schedule_startup_sub = node->create_subscription( rmf_traffic_ros2::ScheduleStartupTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [&](const ScheduleIdentity::SharedPtr msg) { handle_startup_event(*msg); diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp index 1418a86a1..77c3a81e7 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp @@ -344,7 +344,7 @@ void ScheduleNode::setup_changes_services() void ScheduleNode::setup_itinerary_topics() { const auto itinerary_qos = - rclcpp::SystemDefaultsQoS() + rclcpp::SystemDefaultsQoS().keep_last(10) .reliable() .keep_last(100); @@ -400,7 +400,7 @@ void ScheduleNode::setup_incosistency_pub() inconsistency_pub = create_publisher( rmf_traffic_ros2::ScheduleInconsistencyTopicName, - rclcpp::SystemDefaultsQoS().reliable()); + rclcpp::SystemDefaultsQoS().keep_last(10).reliable()); } //============================================================================== @@ -452,7 +452,7 @@ void ScheduleNode::setup_conflict_topics_and_thread() rmf_traffic_ros2::NegotiationConclusionTopicName, negotiation_qos); const auto single_reliable_transient_local = - rclcpp::SystemDefaultsQoS() + rclcpp::SystemDefaultsQoS().keep_last(10) .keep_last(1) .reliable() .transient_local(); @@ -634,7 +634,7 @@ void ScheduleNode::setup_redundancy() start_heartbeat(); const auto permanent_reliable_single_transient_local = - rclcpp::SystemDefaultsQoS() + rclcpp::SystemDefaultsQoS().keep_last(10) .reliable() .keep_last(1) .transient_local(); @@ -656,7 +656,7 @@ void ScheduleNode::setup_redundancy() startup_pub->publish(node_id); const auto reliable_10_transient_local = - rclcpp::SystemDefaultsQoS() + rclcpp::SystemDefaultsQoS().keep_last(10) .reliable() .keep_last(10) .transient_local(); diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp index 4bdbea122..70ebeed67 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp @@ -309,7 +309,7 @@ class Writer::Implementation auto transport = std::shared_ptr(new Transport(node)); const auto itinerary_qos = - rclcpp::SystemDefaultsQoS() + rclcpp::SystemDefaultsQoS().keep_last(10) .reliable() .keep_last(100); @@ -343,7 +343,7 @@ class Writer::Implementation transport->schedule_startup_sub = node->create_subscription( rmf_traffic_ros2::ScheduleStartupTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [w = transport->weak_from_this()](const ScheduleId::SharedPtr msg) { if (const auto self = w.lock()) @@ -367,7 +367,7 @@ class Writer::Implementation transport->inconsistency_sub = node->create_subscription( ScheduleInconsistencyTopicName, - rclcpp::SystemDefaultsQoS().reliable(), + rclcpp::SystemDefaultsQoS().keep_last(10).reliable(), [w = transport->weak_from_this()](const InconsistencyMsg::UniquePtr msg) { if (const auto self = w.lock()) From 5c04a664ae60ec4c7756c81ba0c512566f5fd989 Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Wed, 31 Jan 2024 15:55:32 +0800 Subject: [PATCH 024/119] style Signed-off-by: Teo Koon Peng Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/full_control/main.cpp | 6 ++++-- rmf_fleet_adapter/src/mock_traffic_light/main.cpp | 9 ++++++--- rmf_fleet_adapter/test/test_read_only_adapter.cpp | 3 ++- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index f84f3b03b..6cde76f85 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -1358,11 +1358,13 @@ std::shared_ptr make_fleet( connections->path_request_pub = node->create_publisher< rmf_fleet_msgs::msg::PathRequest>( - rmf_fleet_adapter::PathRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); + rmf_fleet_adapter::PathRequestTopicName, + rclcpp::SystemDefaultsQoS().keep_last(10)); connections->mode_request_pub = node->create_publisher< rmf_fleet_msgs::msg::ModeRequest>( - rmf_fleet_adapter::ModeRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); + rmf_fleet_adapter::ModeRequestTopicName, + rclcpp::SystemDefaultsQoS().keep_last(10)); connections->fleet_state_sub = node->create_subscription< rmf_fleet_msgs::msg::FleetState>( diff --git a/rmf_fleet_adapter/src/mock_traffic_light/main.cpp b/rmf_fleet_adapter/src/mock_traffic_light/main.cpp index ba618ca90..3955f2f25 100644 --- a/rmf_fleet_adapter/src/mock_traffic_light/main.cpp +++ b/rmf_fleet_adapter/src/mock_traffic_light/main.cpp @@ -797,15 +797,18 @@ std::shared_ptr make_fleet( connections->path_request_pub = node->create_publisher< rmf_fleet_msgs::msg::PathRequest>( - rmf_fleet_adapter::PathRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); + rmf_fleet_adapter::PathRequestTopicName, + rclcpp::SystemDefaultsQoS().keep_last(10)); connections->pause_request_pub = node->create_publisher< rmf_fleet_msgs::msg::PauseRequest>( - rmf_fleet_adapter::PauseRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); + rmf_fleet_adapter::PauseRequestTopicName, + rclcpp::SystemDefaultsQoS().keep_last(10)); connections->mode_request_pub = node->create_publisher< rmf_fleet_msgs::msg::ModeRequest>( - rmf_fleet_adapter::ModeRequestTopicName, rclcpp::SystemDefaultsQoS().keep_last(10)); + rmf_fleet_adapter::ModeRequestTopicName, + rclcpp::SystemDefaultsQoS().keep_last(10)); connections->fleet_state_sub = node->create_subscription< rmf_fleet_msgs::msg::FleetState>( diff --git a/rmf_fleet_adapter/test/test_read_only_adapter.cpp b/rmf_fleet_adapter/test/test_read_only_adapter.cpp index 3004e975c..a3340d369 100644 --- a/rmf_fleet_adapter/test/test_read_only_adapter.cpp +++ b/rmf_fleet_adapter/test/test_read_only_adapter.cpp @@ -63,7 +63,8 @@ class TestReadOnly : public rclcpp::Node // subscription to receive String messages which control execution of // this node _cmd_sub = create_subscription( - "test_adapter_" + _fleet_name+"/cmd", rclcpp::SystemDefaultsQoS().keep_last(10), + "test_adapter_" + _fleet_name+"/cmd", + rclcpp::SystemDefaultsQoS().keep_last(10), [&](String::UniquePtr msg) { // receive commands through ros2 msg From a70a6ca2f60a45b130d9f0008e6cc758905cffd3 Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Wed, 31 Jan 2024 16:51:39 +0800 Subject: [PATCH 025/119] catch multi line usages Signed-off-by: Teo Koon Peng Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 4 ++-- rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 6 +++--- .../robot_state_aggregator/RobotStateAggregator.cpp | 2 +- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 4 ++-- .../src/rmf_traffic_ros2/schedule/MirrorManager.cpp | 2 +- .../src/rmf_traffic_ros2/schedule/Node.cpp | 10 +++++----- .../src/rmf_traffic_ros2/schedule/Writer.cpp | 4 ++-- 8 files changed, 17 insertions(+), 17 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index c15346033..77c05fdcb 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -27,8 +27,8 @@ namespace lift_supervisor { Node::Node() : rclcpp::Node("rmf_lift_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); - const auto transient_qos = rclcpp::SystemDefaultsQoS().keep_last(10) + const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); _lift_request_pub = create_publisher( diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index 1e8760238..4f07df44d 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -47,7 +47,7 @@ class Node : public rclcpp::Node Node() : rclcpp::Node("mutex_group_supervisor") { - const auto qos = rclcpp::SystemDefaultsQoS().keep_last(10) + const auto qos = rclcpp::SystemDefaultsQoS() .reliable() .transient_local() .keep_last(100); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 088583428..187be1d46 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -34,9 +34,9 @@ std::shared_ptr Node::make( auto node = std::shared_ptr( new Node(std::move(worker), node_name, options)); - auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + auto default_qos = rclcpp::SystemDefaultsQoS(); default_qos.keep_last(100); - auto transient_qos = rclcpp::SystemDefaultsQoS().keep_last(10) + auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); node->_door_state_obs = @@ -95,7 +95,7 @@ std::shared_ptr Node::make( node->create_publisher( FleetStateTopicName, default_qos); - auto transient_local_qos = rclcpp::SystemDefaultsQoS().keep_last(10) + auto transient_local_qos = rclcpp::SystemDefaultsQoS() .reliable() .transient_local() .keep_last(100); diff --git a/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp b/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp index 772ac0df7..9eba8cd63 100644 --- a/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp +++ b/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp @@ -43,7 +43,7 @@ class RobotStateAggregator : public rclcpp::Node : rclcpp::Node("robot_state_aggregator", options) { RCLCPP_DEBUG(get_logger(), "RobotStateAggregator called"); - const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + const auto default_qos = rclcpp::SystemDefaultsQoS(); const auto state_qos = rclcpp::SystemDefaultsQoS().keep_last(100); #ifdef FAILOVER_MODE diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index cfb2d3ecb..f6801f375 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -261,7 +261,7 @@ class Dispatcher::Implementation // names api_request = node->create_subscription( "task_api_requests", - rclcpp::SystemDefaultsQoS().keep_last(10).reliable().transient_local(), + rclcpp::SystemDefaultsQoS().reliable().transient_local(), [this](const ApiRequestMsg::UniquePtr msg) { this->handle_api_request(*msg); @@ -269,7 +269,7 @@ class Dispatcher::Implementation api_response = node->create_publisher( "task_api_responses", - rclcpp::SystemDefaultsQoS().keep_last(10).reliable().transient_local()); + rclcpp::SystemDefaultsQoS().reliable().transient_local()); // TODO(MXG): The smallest resolution this supports is 1 second. That // doesn't seem great. diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp index 5841ef313..aa294ca25 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp @@ -117,7 +117,7 @@ class MirrorManager::Implementation schedule_startup_sub = node->create_subscription( rmf_traffic_ros2::ScheduleStartupTopicName, - rclcpp::SystemDefaultsQoS().keep_last(10), + rclcpp::SystemDefaultsQoS(), [&](const ScheduleIdentity::SharedPtr msg) { handle_startup_event(*msg); diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp index 77c3a81e7..1418a86a1 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp @@ -344,7 +344,7 @@ void ScheduleNode::setup_changes_services() void ScheduleNode::setup_itinerary_topics() { const auto itinerary_qos = - rclcpp::SystemDefaultsQoS().keep_last(10) + rclcpp::SystemDefaultsQoS() .reliable() .keep_last(100); @@ -400,7 +400,7 @@ void ScheduleNode::setup_incosistency_pub() inconsistency_pub = create_publisher( rmf_traffic_ros2::ScheduleInconsistencyTopicName, - rclcpp::SystemDefaultsQoS().keep_last(10).reliable()); + rclcpp::SystemDefaultsQoS().reliable()); } //============================================================================== @@ -452,7 +452,7 @@ void ScheduleNode::setup_conflict_topics_and_thread() rmf_traffic_ros2::NegotiationConclusionTopicName, negotiation_qos); const auto single_reliable_transient_local = - rclcpp::SystemDefaultsQoS().keep_last(10) + rclcpp::SystemDefaultsQoS() .keep_last(1) .reliable() .transient_local(); @@ -634,7 +634,7 @@ void ScheduleNode::setup_redundancy() start_heartbeat(); const auto permanent_reliable_single_transient_local = - rclcpp::SystemDefaultsQoS().keep_last(10) + rclcpp::SystemDefaultsQoS() .reliable() .keep_last(1) .transient_local(); @@ -656,7 +656,7 @@ void ScheduleNode::setup_redundancy() startup_pub->publish(node_id); const auto reliable_10_transient_local = - rclcpp::SystemDefaultsQoS().keep_last(10) + rclcpp::SystemDefaultsQoS() .reliable() .keep_last(10) .transient_local(); diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp index 70ebeed67..5544a06b8 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp @@ -309,7 +309,7 @@ class Writer::Implementation auto transport = std::shared_ptr(new Transport(node)); const auto itinerary_qos = - rclcpp::SystemDefaultsQoS().keep_last(10) + rclcpp::SystemDefaultsQoS() .reliable() .keep_last(100); @@ -343,7 +343,7 @@ class Writer::Implementation transport->schedule_startup_sub = node->create_subscription( rmf_traffic_ros2::ScheduleStartupTopicName, - rclcpp::SystemDefaultsQoS().keep_last(10), + rclcpp::SystemDefaultsQoS(), [w = transport->weak_from_this()](const ScheduleId::SharedPtr msg) { if (const auto self = w.lock()) From 727dcbd83999b7586ff302c8e3a4b9ff06b1f40a Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Wed, 31 Jan 2024 17:29:05 +0800 Subject: [PATCH 026/119] catch some extra edge cases Signed-off-by: Teo Koon Peng Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 2 +- .../src/robot_state_aggregator/RobotStateAggregator.cpp | 2 +- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 4 ++-- .../src/rmf_traffic_ros2/schedule/MirrorManager.cpp | 2 +- rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp | 2 +- rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 77c05fdcb..e1bb36e60 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -27,7 +27,7 @@ namespace lift_supervisor { Node::Node() : rclcpp::Node("rmf_lift_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); const auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 187be1d46..42cdd06ae 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -34,7 +34,7 @@ std::shared_ptr Node::make( auto node = std::shared_ptr( new Node(std::move(worker), node_name, options)); - auto default_qos = rclcpp::SystemDefaultsQoS(); + auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); default_qos.keep_last(100); auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); diff --git a/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp b/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp index 9eba8cd63..772ac0df7 100644 --- a/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp +++ b/rmf_fleet_adapter/src/robot_state_aggregator/RobotStateAggregator.cpp @@ -43,7 +43,7 @@ class RobotStateAggregator : public rclcpp::Node : rclcpp::Node("robot_state_aggregator", options) { RCLCPP_DEBUG(get_logger(), "RobotStateAggregator called"); - const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); const auto state_qos = rclcpp::SystemDefaultsQoS().keep_last(100); #ifdef FAILOVER_MODE diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index f6801f375..cfb2d3ecb 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -261,7 +261,7 @@ class Dispatcher::Implementation // names api_request = node->create_subscription( "task_api_requests", - rclcpp::SystemDefaultsQoS().reliable().transient_local(), + rclcpp::SystemDefaultsQoS().keep_last(10).reliable().transient_local(), [this](const ApiRequestMsg::UniquePtr msg) { this->handle_api_request(*msg); @@ -269,7 +269,7 @@ class Dispatcher::Implementation api_response = node->create_publisher( "task_api_responses", - rclcpp::SystemDefaultsQoS().reliable().transient_local()); + rclcpp::SystemDefaultsQoS().keep_last(10).reliable().transient_local()); // TODO(MXG): The smallest resolution this supports is 1 second. That // doesn't seem great. diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp index aa294ca25..5841ef313 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp @@ -117,7 +117,7 @@ class MirrorManager::Implementation schedule_startup_sub = node->create_subscription( rmf_traffic_ros2::ScheduleStartupTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [&](const ScheduleIdentity::SharedPtr msg) { handle_startup_event(*msg); diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp index 1418a86a1..629d6cb17 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp @@ -400,7 +400,7 @@ void ScheduleNode::setup_incosistency_pub() inconsistency_pub = create_publisher( rmf_traffic_ros2::ScheduleInconsistencyTopicName, - rclcpp::SystemDefaultsQoS().reliable()); + rclcpp::SystemDefaultsQoS().keep_last(10).reliable()); } //============================================================================== diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp index 5544a06b8..29b973e7e 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Writer.cpp @@ -343,7 +343,7 @@ class Writer::Implementation transport->schedule_startup_sub = node->create_subscription( rmf_traffic_ros2::ScheduleStartupTopicName, - rclcpp::SystemDefaultsQoS(), + rclcpp::SystemDefaultsQoS().keep_last(10), [w = transport->weak_from_this()](const ScheduleId::SharedPtr msg) { if (const auto self = w.lock()) From d4fc0bc20b72ab7d382819496c69245a2f12d3a9 Mon Sep 17 00:00:00 2001 From: Teo Koon Peng Date: Thu, 1 Feb 2024 10:15:36 +0800 Subject: [PATCH 027/119] fix false positive Signed-off-by: Teo Koon Peng Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 42cdd06ae..8664dc562 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -34,8 +34,7 @@ std::shared_ptr Node::make( auto node = std::shared_ptr( new Node(std::move(worker), node_name, options)); - auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); - default_qos.keep_last(100); + auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(100); auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); From e4e94333a3cfd85fbc676cea40beb6530b603c17 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 7 Feb 2024 09:34:56 +0800 Subject: [PATCH 028/119] Change emergency pullover behaviour. Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 15 +++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 21 +++++++++++++++++-- .../events/EmergencyPullover.cpp | 3 ++- .../services/FindEmergencyPullover.cpp | 4 +++- .../services/FindEmergencyPullover.hpp | 4 ++++ .../detail/impl_FindEmergencyPullover.hpp | 2 +- .../services/test_FindEmergencyPullover.cpp | 19 +++++++++++++---- 7 files changed, 59 insertions(+), 9 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index de088aac1..5379e7ef1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -18,6 +18,7 @@ #include "RobotContext.hpp" #include "internal_RobotUpdateHandle.hpp" +#include #include #include @@ -26,6 +27,8 @@ #include #include +#include +#include namespace rmf_fleet_adapter { namespace agv { @@ -1619,5 +1622,17 @@ bool RobotContext::_has_ticket() const { return _reservation_mgr.has_ticket(); } + +//============================================================================== +std::unordered_set RobotContext::_get_free_spots() const { + // TODO(arjo129) Poor efficiency of messages. Change messages to use + // graph/vertex pair. + std::unordered_set set; + for(auto spot: _free_spots.spots) + { + set.insert(std::stoul(spot)); + } + return set; +} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index c81cf88c0..675bcee8d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -45,6 +45,7 @@ #include "../Reporting.hpp" #include "ReservationManager.hpp" #include +#include namespace rmf_fleet_adapter { @@ -714,12 +715,15 @@ class RobotContext /// Release a door. This should only be used by DoorClose void _release_door(const std::string& door_name); - + /// Set an allocated destination. void _set_allocated_destination( const rmf_chope_msgs::msg::ReservationAllocation&); + /// Release last resource that was acquired. std::optional _release_resource(); + std::unordered_set _get_free_spots() const; + /// Has ticket now bool _has_ticket() const; @@ -773,7 +777,16 @@ class RobotContext self->_publish_mutex_group_requests(); }); - //context-> + context->_free_space_sub = context->_node->freespots_obs() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_free_spots = *msg; + }); return context; } @@ -880,10 +893,14 @@ class RobotContext void _publish_mutex_group_requests(); std::unordered_map _requesting_mutex_groups; std::unordered_map _locked_mutex_groups; + + rmf_chope_msgs::msg::FreeParkingSpots _free_spots; + rxcpp::subjects::subject _mutex_group_lock_subject; rxcpp::observable _mutex_group_lock_obs; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; + rmf_rxcpp::subscription_guard _free_space_sub; std::chrono::steady_clock::time_point _last_active_task_time; uint64_t _last_reservation_request_id; ReservationManager _reservation_mgr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 222df2d6f..fa6bb4f4c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -243,7 +243,8 @@ void EmergencyPullover::Active::_find_plan() _state->update_log().info("Searching for an emergency pullover"); _find_pullover_service = std::make_shared( - _context->emergency_planner(), _context->location(), + _context->emergency_planner(), _context->_get_free_spots(), + _context->location(), _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp index 7d2a28b8e..a0420e26c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp @@ -23,6 +23,7 @@ namespace services { //============================================================================== FindEmergencyPullover::FindEmergencyPullover( std::shared_ptr planner, + std::unordered_set free_spots, rmf_traffic::agv::Plan::StartSet starts, std::shared_ptr schedule, rmf_traffic::schedule::ParticipantId participant_id, @@ -31,7 +32,8 @@ FindEmergencyPullover::FindEmergencyPullover( _starts(std::move(starts)), _schedule(std::move(schedule)), _participant_id(participant_id), - _profile(std::move(profile)) + _profile(std::move(profile)), + _parking_spots(std::move(free_spots)) { // Do nothing } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp index 35c597ba0..7d12b7b90 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp @@ -20,6 +20,8 @@ #include "../jobs/SearchForPath.hpp" #include "ProgressEvaluator.hpp" +#include +#include namespace rmf_fleet_adapter { namespace services { @@ -32,6 +34,7 @@ class FindEmergencyPullover FindEmergencyPullover( std::shared_ptr planner, + std::unordered_set free_spots, rmf_traffic::agv::Plan::StartSet starts, std::shared_ptr schedule, rmf_traffic::schedule::ParticipantId participant_id, @@ -53,6 +56,7 @@ class FindEmergencyPullover std::shared_ptr _schedule; rmf_traffic::schedule::ParticipantId _participant_id; std::shared_ptr _profile; + std::unordered_set _parking_spots; std::vector> _search_jobs; rmf_rxcpp::subscription_guard _search_sub; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp index 89e4e5a4d..bf9c7a56b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp @@ -30,7 +30,7 @@ void FindEmergencyPullover::operator()(const Subscriber& s) const auto& graph = _planner->get_configuration().graph(); const std::size_t N = graph.num_waypoints(); _search_jobs.reserve(N); - for (std::size_t i = 0; i < N; ++i) + for (auto i: _parking_spots) { const auto& wp = graph.get_waypoint(i); if (wp.is_parking_spot()) diff --git a/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp b/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp index f63cbc0bb..cb272c325 100644 --- a/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp +++ b/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -117,6 +118,12 @@ SCENARIO("Emergency Pullover") rmf_traffic::agv::Planner::Options{nullptr} ); + std::unordered_set all_waypoints; + for (std::size_t i = 0; i < graph.num_waypoints(); i++) + { + all_waypoints.insert(i); + } + const auto now = std::chrono::steady_clock::now(); using namespace std::chrono_literals; @@ -127,7 +134,8 @@ SCENARIO("Emergency Pullover") auto pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, rmf_traffic::agv::Plan::StartSet({start_0}), + planner, all_waypoints, + rmf_traffic::agv::Plan::StartSet({start_0}), database->snapshot(), p0.id(), std::make_shared(p0.description().profile())); @@ -164,7 +172,8 @@ SCENARIO("Emergency Pullover") // want to park at waypoint 7 in the absence of any other conflicts. pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, rmf_traffic::agv::Plan::StartSet({start_1}), + planner, all_waypoints, + rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); @@ -205,7 +214,8 @@ SCENARIO("Emergency Pullover") pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, rmf_traffic::agv::Plan::StartSet({start_1}), + planner, all_waypoints, + rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); @@ -261,7 +271,8 @@ SCENARIO("Emergency Pullover") auto pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, rmf_traffic::agv::Plan::StartSet({start_1}), + planner, all_waypoints, + rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); From b38612e0907e4551f2d0cf6ec7a0505260cce355 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 7 Feb 2024 11:28:25 +0800 Subject: [PATCH 029/119] Style Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 259 ++++++++++++------ .../rmf_fleet_adapter/StandardNames.hpp | 6 +- .../src/rmf_fleet_adapter/agv/Node.cpp | 6 +- .../src/rmf_fleet_adapter/agv/Node.hpp | 16 +- .../agv/ReservationManager.hpp | 47 ++-- .../rmf_fleet_adapter/agv/RobotContext.cpp | 10 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 2 +- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 13 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 86 ++++-- .../rmf_fleet_adapter/events/GoToPlace.hpp | 9 +- .../phases/ReleaseResource.cpp | 8 +- 11 files changed, 292 insertions(+), 170 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 8f956cd49..a53ce060f 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -21,12 +21,13 @@ const std::string ReservationRequestTopicName = "/rmf/reservations/request"; const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; -const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; +const std::string ReservationAllocationTopicName = + "/rmf/reservations/allocation"; const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; /// C++-isms -template <> +template<> struct std::hash { std::size_t operator()(const rmf_chope_msgs::msg::RequestHeader& header) const @@ -37,24 +38,26 @@ struct std::hash return ((hash()(header.robot_name) - ^ (hash()(header.fleet_name) << 1)) >> 1) - ^ (hash()(header.request_id) << 1); + ^ (hash()(header.fleet_name) << 1)) >> 1) + ^ (hash()(header.request_id) << 1); } }; /// Useful for identifying which tickets belong to which robots. -struct RobotIdentifier { +struct RobotIdentifier +{ std::string robot_name; std::string fleet_name; - bool operator==(const RobotIdentifier& other) const { + bool operator==(const RobotIdentifier& other) const + { return robot_name == other.robot_name && fleet_name == other.fleet_name; } }; -template <> +template<> struct std::hash { std::size_t operator()(const RobotIdentifier& header) const @@ -64,60 +67,69 @@ struct std::hash using std::string; - return ((hash()(header.robot_name) - ^ (hash()(header.fleet_name) << 1)) >> 1); + return (hash()(header.robot_name) + ^ (hash()(header.fleet_name) << 1)) >> 1; } }; /// Ticket generation class for book keeping purposes. Will eventually overflow and leak memory. /// Ticket id 0 does not exist and is useful for making emergency claims. -class TicketStore { +class TicketStore +{ public: - rmf_chope_msgs::msg::Ticket get_new_ticket(const rmf_chope_msgs::msg::RequestHeader &request_header) + rmf_chope_msgs::msg::Ticket get_new_ticket( + const rmf_chope_msgs::msg::RequestHeader& request_header) { rmf_chope_msgs::msg::Ticket ticket; ticket.header = request_header; - ticket.ticket_id = _last_issued_ticket_id; + ticket.ticket_id = _last_issued_ticket_id; RobotIdentifier robot_identifier { request_header.robot_name, request_header.fleet_name }; - _robots_to_tickets[robot_identifier].push_back(_last_issued_ticket_id); + _robots_to_tickets[robot_identifier].push_back(_last_issued_ticket_id); _header_to_ticket.emplace(request_header, _last_issued_ticket_id); _ticket_to_header.emplace(_last_issued_ticket_id, request_header); _last_issued_ticket_id++; return ticket; } - rmf_chope_msgs::msg::Ticket get_existing_ticket(const std::size_t index) - { - rmf_chope_msgs::msg::Ticket ticket; - ticket.ticket_id = index; - ticket.header = _ticket_to_header[index]; - return ticket; - } - - std::unordered_map> _robots_to_tickets; - std::unordered_map _header_to_ticket; - std::unordered_map _ticket_to_header; + rmf_chope_msgs::msg::Ticket get_existing_ticket(const std::size_t index) + { + rmf_chope_msgs::msg::Ticket ticket; + ticket.ticket_id = index; + ticket.header = _ticket_to_header[index]; + return ticket; + } + + std::unordered_map> _robots_to_tickets; + std::unordered_map _header_to_ticket; + std::unordered_map _ticket_to_header; std::size_t _last_issued_ticket_id = 1; }; -struct LocationState { +struct LocationState +{ std::optional ticket; }; -struct LocationReq { +struct LocationReq +{ std::string location; double cost; - bool operator<(const LocationReq& other) const { + bool operator<(const LocationReq& other) const + { return cost < other.cost; } - bool operator==(const LocationReq& other) const { + bool operator==(const LocationReq& other) const + { return cost == other.cost && location == other.location; } }; @@ -125,28 +137,36 @@ struct LocationReq { /// Implements a simple Mutex. Only one robot can claim a location at a time. /// The current implementation is relatively simplistic and basically checks if a location is occupied or not. /// A queuing system is in the works. Note: It is possible for the current system to get deadlocked. -class CurrentState { -public: - std::vector free_locations() { +class CurrentState +{ +public: + std::vector free_locations() + { std::lock_guard lock(_mtx); std::vector locations; - for (auto &[loc, state] : _current_location_reservations) { - if (!state.ticket.has_value()) { + for (auto&[loc, state] : _current_location_reservations) + { + if (!state.ticket.has_value()) + { locations.push_back(loc); } } return locations; } - void add_location(std::string location) { + void add_location(std::string location) + { std::lock_guard lock(_mtx); if (_current_location_reservations.count(location) == 0) { - _current_location_reservations.emplace(location, LocationState {std::nullopt}); + _current_location_reservations.emplace(location, + LocationState {std::nullopt}); } } - - std::optional allocate_lowest_cost_free_spot(const std::vector& incoming_requests, const std::size_t ticket_id) + + std::optional allocate_lowest_cost_free_spot( + const std::vector& incoming_requests, + const std::size_t ticket_id) { if (_ticket_to_location.count(ticket_id) != 0) { @@ -157,14 +177,18 @@ class CurrentState { auto requests = incoming_requests; std::sort(requests.begin(), requests.end()); std::lock_guard lock(_mtx); - for (std::size_t i = 0; i < requests.size(); i++) { + for (std::size_t i = 0; i < requests.size(); i++) + { auto parking = _current_location_reservations.find(requests[i].location); - if (parking == _current_location_reservations.end()) { - _current_location_reservations[requests[i].location] = LocationState {ticket_id}; + if (parking == _current_location_reservations.end()) + { + _current_location_reservations[requests[i].location] = + LocationState {ticket_id}; _ticket_to_location.emplace(ticket_id, requests[i].location); return requests[i].location; } - else if (!parking->second.ticket.has_value()) { + else if (!parking->second.ticket.has_value()) + { _current_location_reservations[requests[i].location].ticket = ticket_id; _ticket_to_location.emplace(ticket_id, requests[i].location); return parking->first; @@ -195,37 +219,62 @@ class CurrentState { using namespace std::chrono_literals; -class SimpleQueueSystemNode : public rclcpp::Node { +class SimpleQueueSystemNode : public rclcpp::Node +{ public: - SimpleQueueSystemNode() : Node("rmf_chope_node") { + SimpleQueueSystemNode() + : Node("rmf_chope_node") + { rclcpp::QoS qos(10); qos = qos.reliable(); qos = qos.keep_last(10); qos = qos.transient_local(); - request_subscription_ = this->create_subscription( - ReservationRequestTopicName, qos, std::bind(&SimpleQueueSystemNode::on_request, this, std::placeholders::_1)); - claim_subscription_ = this->create_subscription( - ReservationClaimTopicName, qos, std::bind(&SimpleQueueSystemNode::claim_request, this, std::placeholders::_1)); - release_subscription_ = this->create_subscription( - ReservationReleaseTopicName, qos, std::bind(&SimpleQueueSystemNode::release, this, std::placeholders::_1)); - graph_subscription_ = this->create_subscription("/nav_graphs", qos, std::bind(&SimpleQueueSystemNode::recieved_graph, this, std::placeholders::_1)); - - ticket_pub_ = this->create_publisher(ReservationResponseTopicName, qos); - allocation_pub_ = this->create_publisher(ReservationAllocationTopicName, qos); - free_spot_pub_ = this->create_publisher("/rmf/reservations/free_parking_spot", qos); - - timer_ = this->create_wall_timer(500ms, std::bind(&SimpleQueueSystemNode::publish_free_spots, this)); + request_subscription_ = + this->create_subscription( + ReservationRequestTopicName, qos, + std::bind(&SimpleQueueSystemNode::on_request, this, + std::placeholders::_1)); + claim_subscription_ = + this->create_subscription( + ReservationClaimTopicName, qos, + std::bind(&SimpleQueueSystemNode::claim_request, this, + std::placeholders::_1)); + release_subscription_ = + this->create_subscription( + ReservationReleaseTopicName, qos, + std::bind(&SimpleQueueSystemNode::release, this, std::placeholders::_1)); + graph_subscription_ = + this->create_subscription( + "/nav_graphs", qos, + std::bind(&SimpleQueueSystemNode::recieved_graph, this, + std::placeholders::_1)); + + ticket_pub_ = this->create_publisher( + ReservationResponseTopicName, qos); + allocation_pub_ = + this->create_publisher( + ReservationAllocationTopicName, qos); + free_spot_pub_ = + this->create_publisher( + "/rmf/reservations/free_parking_spot", qos); + + timer_ = + this->create_wall_timer(500ms, + std::bind(&SimpleQueueSystemNode::publish_free_spots, this)); } private: - void recieved_graph(const rmf_building_map_msgs::msg::Graph::ConstSharedPtr &graph_msg) { + void recieved_graph( + const rmf_building_map_msgs::msg::Graph::ConstSharedPtr& graph_msg) + { RCLCPP_INFO(this->get_logger(), "Got graph"); for (std::size_t i = 0; i < graph_msg->vertices.size(); i++) { - for(auto ¶m: graph_msg->vertices[i].params) { - + for (auto& param: graph_msg->vertices[i].params) + { + //TODO(arjo) make this configure-able if (param.name == "is_parking_spot" && param.value_bool) { @@ -238,11 +287,14 @@ class SimpleQueueSystemNode : public rclcpp::Node { } } } - void on_request(const rmf_chope_msgs::msg::FlexibleTimeRequest::ConstSharedPtr &request) { - + void on_request( + const rmf_chope_msgs::msg::FlexibleTimeRequest::ConstSharedPtr& request) + { + std::vector requests; - for (auto alt: request->alternatives) { + for (auto alt: request->alternatives) + { LocationReq request{ alt.resource_name, alt.cost @@ -256,74 +308,98 @@ class SimpleQueueSystemNode : public rclcpp::Node { ticket_pub_->publish(ticket); } - void claim_request(const rmf_chope_msgs::msg::ClaimRequest::ConstSharedPtr &request) { - + void claim_request( + const rmf_chope_msgs::msg::ClaimRequest::ConstSharedPtr& request) + { + // This logic is for the simplified queue-less version. std::vector locations; - + for (auto claim: requests_[request->ticket.ticket_id]) { locations.push_back(claim); } - - auto cost = (locations.size() == 0) ? 0.0: locations.back().cost + 1.0; - for (auto claim: request->wait_points) { + + auto cost = (locations.size() == 0) ? 0.0 : locations.back().cost + 1.0; + for (auto claim: request->wait_points) + { locations.push_back(LocationReq { - claim, - cost - }); + claim, + cost + }); cost += 1.0; } - auto result = current_state_.allocate_lowest_cost_free_spot(locations, request->ticket.ticket_id); + auto result = current_state_.allocate_lowest_cost_free_spot(locations, + request->ticket.ticket_id); if (result.has_value()) { rmf_chope_msgs::msg::ReservationAllocation allocation; - allocation.ticket = ticket_store_.get_existing_ticket(request->ticket.ticket_id); - allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - + allocation.ticket = ticket_store_.get_existing_ticket( + request->ticket.ticket_id); + allocation.instruction_type = + rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + allocation.resource = result.value(); - for (std::size_t i=0; i < requests_[request->ticket.ticket_id].size(); i++) + for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); + i++) { if (requests_[request->ticket.ticket_id][i].location == result.value()) { - allocation.satisfies_alternative = i; + allocation.satisfies_alternative = i; } } - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", result.value().c_str(), request->ticket.ticket_id); + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + result.value().c_str(), request->ticket.ticket_id); allocation_pub_->publish(allocation); } else { - RCLCPP_INFO(this->get_logger(), "Could not allocate resource for ticket %lu.", request->ticket.ticket_id); + RCLCPP_INFO( + this->get_logger(), "Could not allocate resource for ticket %lu.", + request->ticket.ticket_id); } } - void release(const rmf_chope_msgs::msg::ReleaseRequest::ConstSharedPtr &request) { - RCLCPP_INFO(this->get_logger(), "Releasing ticket for %lu", request->ticket.ticket_id); + void release( + const rmf_chope_msgs::msg::ReleaseRequest::ConstSharedPtr& request) + { + RCLCPP_INFO( + this->get_logger(), "Releasing ticket for %lu", + request->ticket.ticket_id); auto ticket = request->ticket.ticket_id; auto success = current_state_.release(ticket); - if (!success) { - RCLCPP_ERROR(this->get_logger(), "Could not find ticker %lu", request->ticket.ticket_id); + if (!success) + { + RCLCPP_ERROR( + this->get_logger(), "Could not find ticker %lu", + request->ticket.ticket_id); } } - void publish_free_spots() { + void publish_free_spots() + { rmf_chope_msgs::msg::FreeParkingSpots spots; spots.spots = current_state_.free_locations(); free_spot_pub_->publish(spots); } - rclcpp::Subscription::SharedPtr request_subscription_; - rclcpp::Subscription::SharedPtr claim_subscription_; - rclcpp::Subscription::SharedPtr release_subscription_; - rclcpp::Subscription::SharedPtr graph_subscription_; + rclcpp::Subscription::SharedPtr + request_subscription_; + rclcpp::Subscription::SharedPtr + claim_subscription_; + rclcpp::Subscription::SharedPtr + release_subscription_; + rclcpp::Subscription::SharedPtr + graph_subscription_; rclcpp::Publisher::SharedPtr ticket_pub_; - rclcpp::Publisher::SharedPtr allocation_pub_; - rclcpp::Publisher::SharedPtr free_spot_pub_; + rclcpp::Publisher::SharedPtr + allocation_pub_; + rclcpp::Publisher::SharedPtr + free_spot_pub_; std::unordered_map> requests_; TicketStore ticket_store_; @@ -332,7 +408,8 @@ class SimpleQueueSystemNode : public rclcpp::Node { rclcpp::TimerBase::SharedPtr timer_; }; -int main (int argc, const char** argv) { +int main(int argc, const char** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index eb9f62759..f2034d70d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -76,9 +76,11 @@ const std::string MutexGroupStatesTopicName = "mutex_group_states"; const std::string ReservationRequestTopicName = "/rmf/reservations/request"; const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; -const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; +const std::string ReservationAllocationTopicName = + "/rmf/reservations/allocation"; const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; -const std::string ReservationFreeSpotsTopicName = "/rmf/reservations/free_parking_spot"; +const std::string ReservationFreeSpotsTopicName = + "/rmf/reservations/free_parking_spot"; const uint64_t Unclaimed = (uint64_t)(-1); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 8664dc562..36a6ef347 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -128,7 +128,7 @@ std::shared_ptr Node::make( ReservationResponseTopicName, transient_local_qos); node->_reservation_claim_pub = - node->create_publisher( + node->create_publisher( ReservationClaimTopicName, transient_local_qos); node->_reservation_alloc_obs = @@ -137,8 +137,8 @@ std::shared_ptr Node::make( node->_reservation_release_pub = node->create_publisher( - ReservationReleaseTopicName, transient_local_qos); - + ReservationReleaseTopicName, transient_local_qos); + node->_reservation_free_spot_obs = node->create_observable( ReservationFreeSpotsTopicName, transient_local_qos); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 4f25bee9d..81b0945f6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -144,7 +144,8 @@ class Node : public rmf_rxcpp::Transport const MutexGroupStatesObs& mutex_group_states() const; using ReservationRequest = rmf_chope_msgs::msg::FlexibleTimeRequest; - using ReservationRequestPub = rclcpp::Publisher::SharedPtr; + using ReservationRequestPub = + rclcpp::Publisher::SharedPtr; const ReservationRequestPub& location_requester() const; using ReservationTicket = rmf_chope_msgs::msg::Ticket; @@ -152,20 +153,23 @@ class Node : public rmf_rxcpp::Transport const ReservationTicketObs& location_ticket_obs() const; using ReservationClaim = rmf_chope_msgs::msg::ClaimRequest; - using ReservationClaimPub = rclcpp::Publisher::SharedPtr; + using ReservationClaimPub = rclcpp::Publisher::SharedPtr; const ReservationClaimPub& claim_location_ticket() const; using ReservationAllocation = rmf_chope_msgs::msg::ReservationAllocation; - using ReservationAllocationObs = rxcpp::observable; + using ReservationAllocationObs = + rxcpp::observable; const ReservationAllocationObs& allocated_claims_obs() const; using ReservationRelease = rmf_chope_msgs::msg::ReleaseRequest; - using ReservationReleasePub = rclcpp::Publisher::SharedPtr; + using ReservationReleasePub = + rclcpp::Publisher::SharedPtr; const ReservationReleasePub& release_location() const; using ReservationFreeSpotStatus = rmf_chope_msgs::msg::FreeParkingSpots; - using ReservationFreeSpotObs = rxcpp::observable; - const ReservationFreeSpotObs& freespots_obs() const; + using ReservationFreeSpotObs = + rxcpp::observable; + const ReservationFreeSpotObs& freespots_obs() const; template rclcpp::TimerBase::SharedPtr try_create_wall_timer( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index 1e70f78c4..a4fafd227 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -24,35 +24,40 @@ namespace rmf_fleet_adapter { namespace agv { -enum class ReservationState { - AWAITING_REQUEST, - AWAITING_ALLOCATION, - AWAITING_TURN +enum class ReservationState +{ + AWAITING_REQUEST, + AWAITING_ALLOCATION, + AWAITING_TURN }; -class ReservationManager { +class ReservationManager +{ public: - void add_ticket(const rmf_chope_msgs::msg::ReservationAllocation alloc) { - allocations.push_front(alloc); - } - - std::optional release_ticket() { - if (allocations.size() <= 1) { - // For safety every robot must have at least one reservation at any point in time. - return std::nullopt; - } - auto temp = allocations.back(); - allocations.pop_back(); - return temp; - } + void add_ticket(const rmf_chope_msgs::msg::ReservationAllocation alloc) + { + allocations.push_front(alloc); + } - bool has_ticket() const + std::optional release_ticket() + { + if (allocations.size() <= 1) { - return allocations.size() != 0; + // For safety every robot must have at least one reservation at any point in time. + return std::nullopt; } + auto temp = allocations.back(); + allocations.pop_back(); + return temp; + } + + bool has_ticket() const + { + return allocations.size() != 0; + } - std::deque allocations; + std::deque allocations; }; } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 5379e7ef1..3f0da0358 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -894,7 +894,8 @@ rmf_traffic::Duration RobotContext::get_lift_rewait_duration() const } //============================================================================== -uint64_t RobotContext::last_reservation_request_id() { +uint64_t RobotContext::last_reservation_request_id() +{ return _last_reservation_request_id++; } @@ -1612,7 +1613,7 @@ void RobotContext::_set_allocated_destination( //============================================================================== std::optional - RobotContext::_release_resource() +RobotContext::_release_resource() { return _reservation_mgr.release_ticket(); } @@ -1624,11 +1625,12 @@ bool RobotContext::_has_ticket() const } //============================================================================== -std::unordered_set RobotContext::_get_free_spots() const { +std::unordered_set RobotContext::_get_free_spots() const +{ // TODO(arjo129) Poor efficiency of messages. Change messages to use // graph/vertex pair. std::unordered_set set; - for(auto spot: _free_spots.spots) + for (auto spot: _free_spots.spots) { set.insert(std::stoul(spot)); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 675bcee8d..da39cb27c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -723,7 +723,7 @@ class RobotContext std::optional _release_resource(); std::unordered_set _get_free_spots() const; - + /// Has ticket now bool _has_ticket() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 4d39d763a..43cc39cc6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -1296,9 +1296,10 @@ std::optional ExecutePlan::make( rmf_task_sequence::events::Bundle::Type::Sequence, standbys, state, std::move(update))->begin([]() {}, std::move(finished)); - if (waypoint_copy.size() > 0 && context->location().size()) { + if (waypoint_copy.size() > 0 && context->location().size()) + { if (waypoint_copy.back().graph_index() != context->location()[0].waypoint()) - { + { auto allocation = context->_release_resource(); if (allocation.has_value()) { @@ -1309,12 +1310,12 @@ std::optional ExecutePlan::make( msg.ticket = allocation->ticket; context->node()->release_location()->publish(msg); RCLCPP_ERROR( - context->node()->get_logger(), - "Releasing waypoint", - waypoint_copy.size() + context->node()->get_logger(), + "Releasing waypoint", + waypoint_copy.size() ); } - + } } return ExecutePlan{ diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index c04649531..75e9f092a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -169,10 +169,15 @@ auto GoToPlace::Active::make( } active->_reservation_id = active->_context->last_reservation_request_id(); - active->_reservation_ticket = active->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker(active->_context->worker())) - .subscribe([w = active->weak_from_this()](const std::shared_ptr& msg) + active->_reservation_ticket = + active->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( + active->_context->worker())) + .subscribe([w = + active->weak_from_this()](const std::shared_ptr + & + msg) { - + const auto self = w.lock(); if (!self) return; @@ -182,8 +187,8 @@ auto GoToPlace::Active::make( "Got ticket issueing claim"); if (msg->header.request_id != self->_reservation_id - || msg->header.robot_name != self->_context->name() - || msg->header.fleet_name != self->_context->group()) + || msg->header.robot_name != self->_context->name() + || msg->header.fleet_name != self->_context->group()) { return; } @@ -195,24 +200,31 @@ auto GoToPlace::Active::make( auto current_location = self->_context->location(); if (current_location.size() == 0) { - return; + return; } - // Order wait points by the distance from the destination. + // Order wait points by the distance from the destination. std::vector> waitpoints_order; - for (std::size_t wp_idx = 0; wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) { - const auto wp = self->_context->navigation_graph().get_waypoint(wp_idx); + for (std::size_t wp_idx = 0; + wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) + { + const auto wp = self->_context->navigation_graph().get_waypoint( + wp_idx); // Wait at parking spot and check its on same floor. - if (!wp.is_parking_spot() || wp.get_map_name() != self->_context->map()) { + if (!wp.is_parking_spot() || + wp.get_map_name() != self->_context->map()) + { continue; } - - auto result = self->_context->planner()->quickest_path(current_location, wp_idx); - if (!result.has_value()) { + + auto result = + self->_context->planner()->quickest_path(current_location, wp_idx); + if (!result.has_value()) + { continue; } - + std::stringstream json_stream; json_stream << wp_idx << std::endl; std::string json; @@ -220,7 +232,10 @@ auto GoToPlace::Active::make( waitpoints_order.emplace_back(result->cost(), json); } - std::sort(waitpoints_order.begin(), waitpoints_order.end(), [](const std::pair& a, const std::pair& b) { + std::sort(waitpoints_order.begin(), waitpoints_order.end(), + [](const std::pair& a, const std::pair& b) + { return a.first < b.first; }); @@ -228,17 +243,23 @@ auto GoToPlace::Active::make( rmf_chope_msgs::msg::ClaimRequest claim_request; claim_request.ticket = *msg; std::vector waitpoints; - for (auto &[_, waitpoint]: waitpoints_order) { - claim_request.wait_points.push_back(waitpoint); - } + for (auto&[_, waitpoint]: waitpoints_order) + { + claim_request.wait_points.push_back(waitpoint); + } self->_context->node()->claim_location_ticket()->publish(claim_request); RCLCPP_ERROR( - self->_context->node()->get_logger(), + self->_context->node()->get_logger(), "Claim issued"); }); - active->_reservation_allocation = active->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker(active->_context->worker())) - .subscribe([w = active->weak_from_this()](const std::shared_ptr& msg) + active->_reservation_allocation = + active->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( + active->_context->worker())) + .subscribe([w = + active->weak_from_this()](const std::shared_ptr + & + msg) { const auto self = w.lock(); if (!self) @@ -257,7 +278,7 @@ auto GoToPlace::Active::make( self->_final_allocated_destination = msg; self->_current_reservation_state = ReservationState::RecievedResponse; }); - + active->_negotiator = Negotiator::make( @@ -521,8 +542,8 @@ std::optional GoToPlace::Active::_choose_goal( // No need to use reservation system if we are already there. if (_description.one_of().size() == 1 - && _description.one_of()[0].waypoint() == current_location[0].waypoint() - && _context->_has_ticket()) + && _description.one_of()[0].waypoint() == current_location[0].waypoint() + && _context->_has_ticket()) { return _description.one_of()[0]; } @@ -560,7 +581,8 @@ std::optional GoToPlace::Active::_choose_goal( } // Find distance to said point - auto result = _context->planner()->quickest_path(current_location, wp_idx); + auto result = + _context->planner()->quickest_path(current_location, wp_idx); if (result.has_value()) { RCLCPP_INFO( @@ -579,7 +601,7 @@ std::optional GoToPlace::Active::_choose_goal( json_stream << wp_idx << std::endl; std::string json; json_stream >> json; - + rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; alternative.resource_name = json; alternative.cost = result->cost(); @@ -609,14 +631,18 @@ std::optional GoToPlace::Active::_choose_goal( else if (_current_reservation_state == ReservationState::RecievedResponse) { // Go to recommended destination - if (_final_allocated_destination.value()->instruction_type == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { - _context->_set_allocated_destination(*_final_allocated_destination.value().get()); - return _description.one_of()[_final_allocated_destination.value()->satisfies_alternative]; + if (_final_allocated_destination.value()->instruction_type == + rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + { + _context->_set_allocated_destination( + *_final_allocated_destination.value().get()); + return _description.one_of()[_final_allocated_destination.value()-> + satisfies_alternative]; } // For queueing system in future //return rmf_traffic::agv::Plan::Goal(std::stoul(_final_allocated_destination.value()->resource), std::nullopt); - // For now just keep retrying until your turn. + // For now just keep retrying until your turn. // Probably not needed since, the node does not publish an allocation till its available? RCLCPP_ERROR( _context->node()->get_logger(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index fd0a31a02..9187f5a0e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -106,7 +106,8 @@ class GoToPlace : public rmf_task_sequence::Event private: - enum class ReservationState { + enum class ReservationState + { Pending=0, Requested=1, RecievedResponse=2 @@ -157,8 +158,10 @@ class GoToPlace : public rmf_task_sequence::Event rmf_rxcpp::subscription_guard _reservation_allocation; uint64_t _reservation_id = 0; - std::optional> _ticket{std::nullopt}; - std::optional> _final_allocated_destination{std::nullopt}; + std::optional> _ticket{std:: + nullopt}; + std::optional> + _final_allocated_destination{std::nullopt}; bool _is_interrupted = false; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp index a877334d6..8f197ce91 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp @@ -52,7 +52,8 @@ ReleaseResource::ActivePhase::observe() const } //============================================================================== -rmf_traffic::Duration ReleaseResource::ActivePhase::estimate_remaining_time() const +rmf_traffic::Duration ReleaseResource::ActivePhase::estimate_remaining_time() +const { // TODO: implement return rmf_traffic::Duration{0}; @@ -79,7 +80,7 @@ const std::string& ReleaseResource::ActivePhase::description() const //============================================================================== void ReleaseResource::ActivePhase::_init_obs() { - + } //============================================================================== @@ -117,7 +118,8 @@ std::shared_ptr ReleaseResource::PendingPhase::begin() } //============================================================================== -rmf_traffic::Duration ReleaseResource::PendingPhase::estimate_phase_duration() const +rmf_traffic::Duration ReleaseResource::PendingPhase::estimate_phase_duration() +const { // TODO: implement return rmf_traffic::Duration{0}; From 3af172e482e749e67868a3f6a706460ed6aa28cf Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Feb 2024 17:08:22 +0800 Subject: [PATCH 030/119] Remove holding time nonsense. Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 14481a579..5a24d0bfc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1408,10 +1408,6 @@ PlaceDeserializer make_place_deserializer( const auto& ori_it = msg.find("orientation"); if (ori_it != msg.end()) place->orientation(ori_it->get()); - - const auto& hold_it = msg.find("hold_for"); - if (hold_it != msg.end()) - place->holding_time(hold_it->get()); } return {place, {}}; From 317ee7fb557177f1113f946e9e53c7c6d025b871 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 1 Mar 2024 11:20:46 +0800 Subject: [PATCH 031/119] Don't wait for retry. immediately plan. Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 2 + .../phases/ReleaseResource.cpp | 135 ------------------ .../phases/ReleaseResource.hpp | 104 -------------- 3 files changed, 2 insertions(+), 239 deletions(-) delete mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp delete mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 75e9f092a..2259ecec8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -277,6 +277,8 @@ auto GoToPlace::Active::make( self->_final_allocated_destination = msg; self->_current_reservation_state = ReservationState::RecievedResponse; + self->_retry_timer->cancel(); + self->_find_plan(); }); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp deleted file mode 100644 index 8f197ce91..000000000 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/* - * Copyright (C) 2020 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 "ReleaseResource.hpp" -#include "RxOperators.hpp" -#include "SupervisorHasSession.hpp" -#include "rmf_fleet_adapter/StandardNames.hpp" - -namespace rmf_fleet_adapter { -namespace phases { - -//============================================================================== -std::shared_ptr ReleaseResource::ActivePhase::make( - agv::RobotContextPtr context, - std::string door_name, - std::string request_id) -{ - RCLCPP_INFO( - context->node()->get_logger(), - "Releasing [%s] for [%s]", - door_name.c_str(), - context->requester_id().c_str()); - context->_release_door(door_name); - auto inst = std::shared_ptr(new ActivePhase( - std::move(context), - std::move(door_name), - std::move(request_id) - )); - inst->_init_obs(); - return inst; -} - -//============================================================================== -const rxcpp::observable& -ReleaseResource::ActivePhase::observe() const -{ - return _obs; -} - -//============================================================================== -rmf_traffic::Duration ReleaseResource::ActivePhase::estimate_remaining_time() -const -{ - // TODO: implement - return rmf_traffic::Duration{0}; -} - -//============================================================================== -void ReleaseResource::ActivePhase::emergency_alarm(bool /*on*/) -{ - // TODO: implement -} - -//============================================================================== -void ReleaseResource::ActivePhase::cancel() -{ - // Don't actually cancel anything here. We just release the space. -} - -//============================================================================== -const std::string& ReleaseResource::ActivePhase::description() const -{ - return _description; -} - -//============================================================================== -void ReleaseResource::ActivePhase::_init_obs() -{ - -} - -//============================================================================== -void ReleaseResource::ActivePhase::_publish_release_spot() -{ -} - -//============================================================================== -ReleaseResource::ActivePhase::ActivePhase( - agv::RobotContextPtr context, - std::string door_name, - std::string request_id) -: _context(std::move(context)), - _door_name(std::move(door_name)), - _request_id(std::move(request_id)) -{ - _description = "Closing [door:" + _door_name + "]"; -} - -//============================================================================== -ReleaseResource::PendingPhase::PendingPhase( - agv::RobotContextPtr context) -: _context(std::move(context)) -{ - _description = "Releasing Resource"; -} - -//============================================================================== -std::shared_ptr ReleaseResource::PendingPhase::begin() -{ - return ReleaseResource::ActivePhase::make( - _context, - _door_name, - _request_id); -} - -//============================================================================== -rmf_traffic::Duration ReleaseResource::PendingPhase::estimate_phase_duration() -const -{ - // TODO: implement - return rmf_traffic::Duration{0}; -} - -//============================================================================== -const std::string& ReleaseResource::PendingPhase::description() const -{ - return _description; -} - -} // namespace phases -} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp deleted file mode 100644 index 4c7865628..000000000 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ReleaseResource.hpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright (C) 2020 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 SRC__RMF_FLEET_ADAPTER__PHASES__RELEASE_RESOURCE_HPP -#define SRC__RMF_FLEET_ADAPTER__PHASES__RELEASE_RESOURCE_HPP - -#include "../LegacyTask.hpp" -#include "../agv/RobotContext.hpp" - -#include -#include - -namespace rmf_fleet_adapter { -namespace phases { - -struct ReleaseResource -{ - /** - * The phase should do the following - * 1. Send out a MODE_CLOSED door request - * 2. Periodically resend the close request while the supervisor state contains the requester_id - * 3. It is completed when the supervisor state does NOT contains the requester_id, regardless of the door state - * 4. Cancellation requests are ignored - */ - class ActivePhase : public LegacyTask::ActivePhase, - public std::enable_shared_from_this - { - public: - - static std::shared_ptr make( - agv::RobotContextPtr context, - std::string door_name, - std::string request_id); - - const rxcpp::observable& observe() const override; - - rmf_traffic::Duration estimate_remaining_time() const override; - - void emergency_alarm(bool on) override; - - void cancel() override; - - const std::string& description() const override; - - private: - - agv::RobotContextPtr _context; - std::string _door_name; - std::string _request_id; - rxcpp::observable _obs; - std::string _description; - rclcpp::TimerBase::SharedPtr _timer; - LegacyTask::StatusMsg _status; - - ActivePhase( - agv::RobotContextPtr context, - std::string door_name, - std::string request_id); - - void _init_obs(); - - void _publish_release_spot(); - }; - - class PendingPhase : public LegacyTask::PendingPhase - { - public: - - PendingPhase( - agv::RobotContextPtr context); - - std::shared_ptr begin() override; - - rmf_traffic::Duration estimate_phase_duration() const override; - - const std::string& description() const override; - - private: - - agv::RobotContextPtr _context; - std::string _door_name; - std::string _request_id; - std::string _description; - }; -}; - -} // namespace phases -} // namespace rmf_fleet_adapter - -#endif // SRC__RMF_FLEET_ADAPTER__PHASES__DOORCLOSE_HPP From 98d3b37725db7a87f92a4775049689b1278c0077 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 1 Mar 2024 11:55:58 +0800 Subject: [PATCH 032/119] Cancel retry timer Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 2259ecec8..d615a5c28 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -870,6 +870,7 @@ void GoToPlace::Active::_stop_and_clear() if (const auto command = _context->command()) command->stop(); + _retry_timer->cancel(); _context->itinerary().clear(); } From dd65259d286fa873e5d4b24be19af396b61cbe77 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 1 Mar 2024 13:59:03 +0800 Subject: [PATCH 033/119] Better logging. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index a53ce060f..8271eb2a6 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -17,6 +17,7 @@ #include #include +#include const std::string ReservationRequestTopicName = "/rmf/reservations/request"; const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; @@ -171,6 +172,7 @@ class CurrentState if (_ticket_to_location.count(ticket_id) != 0) { // Ticket has been allocated. Probably some DDS-ism causing the issue + std::cerr << "Ticket already allocated" << std::endl; return std::nullopt; } @@ -195,6 +197,12 @@ class CurrentState } } + std::cerr << "Could not find route to any of: "; + for (auto c: requests) { + std::cerr << "" << c.location <<", "; + } + std::cerr << "\n"; + return std::nullopt; } @@ -275,7 +283,7 @@ class SimpleQueueSystemNode : public rclcpp::Node for (auto& param: graph_msg->vertices[i].params) { - //TODO(arjo) make this configure-able + //TODO(arjoc) make this configure-able if (param.name == "is_parking_spot" && param.value_bool) { std::stringstream name; From bcde5fe040362f87d8c0ab3a632ca8a61b8793c3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 26 Apr 2024 11:51:06 +0800 Subject: [PATCH 034/119] merge Signed-off-by: Arjo Chakravarty --- .../client/ClientWebSocketEndpoint.cpp | 253 ++++++++++++++++++ 1 file changed, 253 insertions(+) create mode 100644 rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp diff --git a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp new file mode 100644 index 000000000..99a823ba4 --- /dev/null +++ b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp @@ -0,0 +1,253 @@ +#include "ClientWebSocketEndpoint.hpp" +#include +#include + +using namespace std::chrono_literals; +using namespace rmf_websocket; +using namespace boost; + +//============================================================================= +ConnectionMetadata::ConnectionMetadata( + websocketpp::connection_hdl hdl, std::string uri, ConnectionCallback cb, + ReconnectionCallback rcb) +: _hdl(hdl) + , _status(ConnectionStatus::CONNECTING) + , _uri(uri) + , _server("N/A") + , _connection_cb(cb) + , _reconnection_cb(std::move(rcb)) +{ +} + +//============================================================================= +void ConnectionMetadata::reset() +{ + _status = ConnectionStatus::CONNECTING; +} + +//============================================================================= +void ConnectionMetadata::on_open(WsClient* c, websocketpp::connection_hdl hdl) +{ + _status = ConnectionStatus::OPEN; + WsClient::connection_ptr con = c->get_con_from_hdl(hdl); + _server = con->get_response_header("Server"); + _connection_cb(); +} + +//============================================================================= +void ConnectionMetadata::on_fail(WsClient* c, websocketpp::connection_hdl hdl) +{ + _status = ConnectionStatus::FAILED; + WsClient::connection_ptr con = c->get_con_from_hdl(hdl); + _server = con->get_response_header("Server"); + _error_reason = con->get_ec().message(); + c->get_io_service().post(_reconnection_cb); +} + +//============================================================================= +void ConnectionMetadata::on_close(WsClient* c, websocketpp::connection_hdl hdl) +{ + _status = ConnectionStatus::CLOSED; + WsClient::connection_ptr con = c->get_con_from_hdl(hdl); + std::stringstream s; + s << "close code: " << con->get_remote_close_code() << " (" + << websocketpp::close::status::get_string(con->get_remote_close_code()) + << "), close reason: " << con->get_remote_close_reason(); + _error_reason = s.str(); + c->get_io_service().post(_reconnection_cb); +} + +//============================================================================= +std::string ConnectionMetadata::debug_data() const +{ + std::stringstream out; + std::string status_string; + + switch (_status) + { + case ConnectionStatus::CONNECTING: + status_string = "Connecting"; + break; + case ConnectionStatus::OPEN: + status_string = "Open"; + break; + case ConnectionStatus::CLOSED: + status_string = "Closed"; + break; + case ConnectionStatus::FAILED: + status_string = "Closed"; + break; + default: + status_string = "Unknown"; + break; + } + + + out << "> URI: " << _uri << "\n" + << "> Status: " << status_string << "\n" + << "> Remote Server: " + << (_server.empty() ? "None Specified" : _server) << "\n" + << "> Error/close reason: " + << (_error_reason.empty() ? "N/A" : _error_reason) << "\n"; + + return out.str(); +} + +//============================================================================= +ConnectionMetadata::ConnectionStatus ConnectionMetadata::get_status() const +{ + return _status; +} + +//============================================================================= +websocketpp::connection_hdl ConnectionMetadata::get_hdl() const +{ + return _hdl; +} + +//============================================================================= +ClientWebSocketEndpoint::ClientWebSocketEndpoint( + std::string const& uri, std::shared_ptr node, + asio::io_service* io_service, + ConnectionCallback cb) +: _uri(uri), _node(node), _init{false}, _reconnect_enqueued(false), + _connection_cb(std::move(cb)) +{ + _endpoint = std::make_unique(); + _endpoint->clear_access_channels(websocketpp::log::alevel::all); + _endpoint->clear_error_channels(websocketpp::log::elevel::all); + _endpoint->init_asio(io_service); + _endpoint->start_perpetual(); +} + +//============================================================================= +websocketpp::lib::error_code ClientWebSocketEndpoint::connect() +{ + websocketpp::lib::error_code ec; + + _init = true; + _con = _endpoint->get_connection(_uri, ec); + RCLCPP_INFO(_node->get_logger(), "Attempting to connect to %s", _uri.c_str()); + + if (ec) + { + RCLCPP_ERROR(_node->get_logger(), "> Connect initialization error: %s\n" + "> Host: %s\n", ec.message().c_str(), _uri.c_str()); + return ec; + } + + auto reconnect_socket = [this]() + { + // TODO(arjo) Parametrize the timeout. + RCLCPP_ERROR(_node->get_logger(), + "Connection lost\n" + "> Reconnecting in 1s\n" + "> Host: %s", _uri.c_str()); + _endpoint->stop_perpetual(); + auto io_service = &_endpoint->get_io_service(); + _endpoint = std::make_unique(); + _endpoint->clear_access_channels(websocketpp::log::alevel::all); + _endpoint->clear_error_channels(websocketpp::log::elevel::all); + _endpoint->init_asio(io_service); + _endpoint->start_perpetual(); + websocketpp::lib::error_code ec; + + _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, + this)); + + }; + auto connected_cb = [this]() + { + _connection_cb(); + }; + + // Not sure why but seems like I have to re-initallize this everytime in order + // to actually make a clean connection. My guess is the shared pointer is being + // leaked somewhere. + _current_connection = std::make_shared( + _con->get_handle(), + _uri, connected_cb, reconnect_socket); + + _con->set_open_handler([this](websocketpp::connection_hdl hdl) + { + RCLCPP_INFO(_node->get_logger(), "Succesfully connected to %s", + _uri.c_str()); + _current_connection->on_open(_endpoint.get(), hdl); + }); + _con->set_fail_handler([this](websocketpp::connection_hdl hdl) + { + _current_connection->on_fail(_endpoint.get(), hdl); + RCLCPP_ERROR(_node->get_logger(), "Connection to %s failed. Reason:\n %s", + _uri.c_str(), _current_connection->debug_data().c_str()); + }); + + _con->set_close_handler([this](websocketpp::connection_hdl hdl) + { + _current_connection->on_close(_endpoint.get(), hdl); + RCLCPP_INFO(_node->get_logger(), "Connection to %s closed. Reason:\n %s", + _uri.c_str(), _current_connection->debug_data().c_str()); + }); + + + _endpoint->connect(_con); + + return ec; +} + +//============================================================================= +std::optional ClientWebSocketEndpoint:: +get_status() const +{ + if (!_init) + { + return std::nullopt; + } + return _current_connection->get_status(); +} + +//============================================================================= +websocketpp::lib::error_code ClientWebSocketEndpoint::send( + const std::string& message) +{ + websocketpp::lib::error_code ec; + + _endpoint->send( + _current_connection->get_hdl(), message, websocketpp::frame::opcode::text, + ec); + if (ec) + { + return ec; + } + return ec; +} + +//============================================================================= +ClientWebSocketEndpoint::~ClientWebSocketEndpoint() +{ + _endpoint->stop_perpetual(); + + if (!_current_connection) + { + return; + } + + if (_current_connection->get_status() != + ConnectionMetadata::ConnectionStatus::OPEN) + { + // Only close open connections + return; + } + + + websocketpp::lib::error_code ec; + _endpoint->close( + _current_connection->get_hdl(), websocketpp::close::status::going_away, "", + ec); + if (ec) + { + RCLCPP_ERROR( + _node->get_logger(), "Error closing connection: %s\n. Host: %s", + ec.message().c_str(), _uri.c_str()); + } + +} From dfbcdfe9a4873de58c2aa494828f724090c84f5a Mon Sep 17 00:00:00 2001 From: cwrx777 Date: Wed, 28 Feb 2024 17:02:01 +0800 Subject: [PATCH 035/119] add in_lift readonly property in Graph::Waypoint binding. (#326) Signed-off-by: Charly Wu Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter_python/src/graph/graph.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index d65f880de..0aaf60172 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -70,7 +70,8 @@ void bind_graph(py::module& m) &Graph::Waypoint::set_charger) .def("set_charger", &Graph::Waypoint::set_charger) .def_property_readonly("index", &Graph::Waypoint::index) - .def_property_readonly("waypoint_name", &Graph::Waypoint::name); + .def_property_readonly("waypoint_name", &Graph::Waypoint::name) + .def_property_readonly("in_lift", &Graph::Waypoint::in_lift); // ORIENTATION_CONSTRAINT ==================================================== py::class_ Date: Wed, 6 Mar 2024 12:42:29 +0800 Subject: [PATCH 036/119] Add Backward-ROS for improved logging in event of segfaults (#327) * Add Backward-ROS for improved logging in event of segfaults `backward-ros` provides a clean way to catch unhandled exceptions and segfaults and is able to print a stack trace when a program crashes. This PR adds `backward-ros` to rmf ros2 nodes so that we can trace causes for segfaults. Note: This will only work for pure C++ nodes. If we want logging in downstream fleetadapters they will have to handle that themselves either using `backward-cpp` or `faulthandler` in python. Signed-off-by: Arjo Chakravarty * Alphabetize deps Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty Co-authored-by: Yadu --- rmf_fleet_adapter/CMakeLists.txt | 1 + rmf_fleet_adapter/package.xml | 33 +++++++++++++++++--------------- rmf_task_ros2/CMakeLists.txt | 1 + rmf_task_ros2/package.xml | 13 +++++++------ rmf_traffic_ros2/CMakeLists.txt | 1 + rmf_traffic_ros2/package.xml | 17 ++++++++-------- 6 files changed, 37 insertions(+), 29 deletions(-) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 152250741..7ae87b9f0 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -11,6 +11,7 @@ add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(Eigen3 REQUIRED) find_package(yaml-cpp REQUIRED) find_package(Threads REQUIRED) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index ffbcdc5a6..8e2547848 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -12,31 +12,34 @@ ament_cmake - rclcpp + + backward_ros + nlohmann_json_schema_validator_vendor + nlohmann-json-dev rclcpp_components - rmf_utils - rmf_door_msgs - rmf_ingestor_msgs + rclcpp + rmf_api_msgs + rmf_battery + rmf_building_map_msgs + rmf_chope_msgs rmf_dispenser_msgs + rmf_door_msgs rmf_fleet_msgs + rmf_ingestor_msgs rmf_lift_msgs rmf_task_msgs - rmf_traffic - rmf_traffic_ros2 rmf_task_ros2 - rmf_battery - rmf_task rmf_task_sequence + rmf_task + rmf_traffic_ros2 + rmf_traffic + rmf_utils + rmf_websocket std_msgs - rmf_api_msgs - rmf_building_map_msgs - rmf_chope_msgs - stubborn_buddies + stubborn_buddies_msgs + stubborn_buddies - rmf_websocket - nlohmann-json-dev - nlohmann_json_schema_validator_vendor eigen yaml-cpp diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt index 1fbeca571..1ded00bd2 100644 --- a/rmf_task_ros2/CMakeLists.txt +++ b/rmf_task_ros2/CMakeLists.txt @@ -14,6 +14,7 @@ endif() include(GNUInstallDirs) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(rmf_api_msgs REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_traffic_ros2 REQUIRED) diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index fcc39c053..eda399dad 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -10,15 +10,16 @@ ament_cmake - rmf_utils + backward_ros + nlohmann_json_schema_validator_vendor + nlohmann-json-dev + rclcpp rmf_api_msgs - rmf_traffic - rmf_traffic_ros2 rmf_task_msgs - rclcpp + rmf_traffic_ros2 + rmf_traffic + rmf_utils rmf_websocket - nlohmann-json-dev - nlohmann_json_schema_validator_vendor eigen diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 341c098c1..bc135fd83 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -16,6 +16,7 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR}/cmake) include(GNUInstallDirs) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(rmf_traffic 3 REQUIRED) find_package(rmf_traffic_msgs REQUIRED) find_package(rmf_site_map_msgs REQUIRED) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index eff1187b5..e651c9b16 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -11,17 +11,18 @@ ament_cmake - rmf_utils - rmf_traffic - rmf_traffic_msgs - rmf_fleet_msgs + backward_ros + nlohmann-json-dev + proj + rclcpp + rmf_building_map_msgs rmf_chope_msgs + rmf_fleet_msgs rmf_site_map_msgs - rmf_building_map_msgs - rclcpp + rmf_traffic_msgs + rmf_traffic + rmf_utils yaml-cpp - nlohmann-json-dev - proj zlib uuid From ea0afb36332d242f0947c10be2a3623cfcad19ac Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 13 Mar 2024 14:41:28 +0800 Subject: [PATCH 037/119] include cstdint header (#331) Signed-off-by: Yadunund Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index f2034d70d..310812928 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -18,6 +18,7 @@ #ifndef RMF_FLEET_ADAPTER__STANDARDNAMES_HPP #define RMF_FLEET_ADAPTER__STANDARDNAMES_HPP +#include #include namespace rmf_fleet_adapter { From ccba1e53e72602d6fb380bfac2cb4b20a66b6835 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 13 Mar 2024 14:44:47 +0800 Subject: [PATCH 038/119] Removes a line of dead code (#322) Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 43cc39cc6..7887629ad 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -960,7 +960,6 @@ std::optional ExecutePlan::make( return std::make_pair(mutex_group_change, new_mutex_groups); }; - const auto t0 = waypoints.front().time(); while (!waypoints.empty()) { auto it = waypoints.begin(); From b10baeec954515f6205742c478f7b9f72309824f Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 13 Mar 2024 15:09:57 +0800 Subject: [PATCH 039/119] Bump main to 2.6.0 (#332) * Update changelogs Signed-off-by: Yadunund * 2.6.0 Signed-off-by: Yadunund --------- Signed-off-by: Yadunund Signed-off-by: Arjo Chakravarty --- rmf_charging_schedule/CHANGELOG.rst | 3 +++ rmf_charging_schedule/package.xml | 2 +- rmf_charging_schedule/setup.py | 2 +- rmf_fleet_adapter/CHANGELOG.rst | 10 ++++++++++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 5 +++++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 7 +++++++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 6 ++++++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 3 +++ rmf_websocket/package.xml | 2 +- 13 files changed, 41 insertions(+), 7 deletions(-) diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst index 9215a03c4..a35145359 100644 --- a/rmf_charging_schedule/CHANGELOG.rst +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_charging_schedule ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.0 (2024-03-13) +------------------ + 2.5.0 (2023-12-22) ------------------ diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index 84d034d8f..826e46512 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -2,7 +2,7 @@ rmf_charging_schedule - 2.5.0 + 2.6.0 Node for a fixed 24-hour rotating charger usage schedule Grey Apache License 2.0 diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py index 55754a0d6..bee67625b 100644 --- a/rmf_charging_schedule/setup.py +++ b/rmf_charging_schedule/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version='2.5.0', + version='2.6.0', packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index 0062e4071..2c9482feb 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.0 (2024-03-13) +------------------ +* Removes a line of dead code (`#322 `_) +* include cstdint header (`#331 `_) +* Add Backward-ROS for improved logging in event of segfaults (`#327 `_) +* Explicitly specify all qos depth (`#323 `_) +* Add support of fleet-level task (`#317 `_) +* Fix minor logging error (`#318 `_) +* Contributors: Arjo Chakravarty, Teo Koon Peng, Yadunund, cwrx777 + 2.5.0 (2023-12-22) ------------------ * Fix edge case when starting on a lane (`#312 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 8e2547848..8efedb51e 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.5.0 + 2.6.0 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index 9191dcdf4..48c1ea0c7 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.0 (2024-03-13) +------------------ +* add in_lift readonly property in Graph::Waypoint binding. (`#326 `_) +* Contributors: cwrx777 + 2.5.0 (2023-12-22) ------------------ diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index 16a4e4dea..3f345e84a 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.5.0 + 2.6.0 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index 250fac739..ec86b9041 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.0 (2024-03-13) +------------------ +* Add Backward-ROS for improved logging in event of segfaults (`#327 `_) +* Explicitly specify all qos depth (`#323 `_) +* Adds an option to generate unique hex strings for new task ids, fix time stamp logic (`#319 `_) +* Contributors: Aaron Chong, Arjo Chakravarty, Teo Koon Peng + 2.5.0 (2023-12-22) ------------------ diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index eda399dad..5122c5251 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.5.0 + 2.6.0 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index c3169e89f..4648c0459 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.0 (2024-03-13) +------------------ +* Add Backward-ROS for improved logging in event of segfaults (`#327 `_) +* Explicitly specify all qos depth (`#323 `_) +* Contributors: Arjo Chakravarty, Teo Koon Peng, Yadunund + 2.5.0 (2023-12-22) ------------------ diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index e651c9b16..5d3cfb3d3 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.5.0 + 2.6.0 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index 8ef63be1a..dcae2c666 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.6.0 (2024-03-13) +------------------ + 2.5.0 (2023-12-22) ------------------ diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index a884dac6a..6919de27c 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.5.0 + 2.6.0 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From 145b9cf71712f7ea558b8bc28f01b4c247eca7db Mon Sep 17 00:00:00 2001 From: Yadu Date: Mon, 18 Mar 2024 09:45:04 +0800 Subject: [PATCH 040/119] Add Speed Limit Requests (#222) (#335) * add FleetUpdateHandle speed limit API to python bindings * add continuous checker option for dynamic obstacles * refactor Lane Close and Open msg building * add speed limits * Fix style * Fix const correctness and do not pass unordered_map by ref to publisher Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Signed-off-by: Yadunund Co-authored-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com> Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter_python/src/adapter.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 99c028e5b..0381dd164 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -10,6 +10,7 @@ #include "rmf_traffic_ros2/Time.hpp" #include "rmf_fleet_adapter/agv/Adapter.hpp" #include "rmf_fleet_adapter/agv/EasyFullControl.hpp" +#include "rmf_fleet_adapter/agv/FleetUpdateHandle.hpp" #include "rmf_fleet_adapter/agv/test/MockAdapter.hpp" #include "rmf_fleet_adapter_python/PyRobotCommandHandle.hpp" #include @@ -391,6 +392,12 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("open_lanes", &agv::FleetUpdateHandle::open_lanes, py::arg("lane_indices")) + .def("limit_lane_speeds", + &agv::FleetUpdateHandle::limit_lane_speeds, + py::arg("requests")) + .def("remove_speed_limits", + &agv::FleetUpdateHandle::remove_speed_limits, + py::arg("requests")) .def("set_task_planner_params", [&](agv::FleetUpdateHandle& self, battery::BatterySystem& b_sys, @@ -590,6 +597,14 @@ PYBIND11_MODULE(rmf_adapter, m) { return self.errors(); }); + // SPEED LIMIT REQUEST =============================================== + py::class_( + m_fleet_update_handle, "SpeedLimitRequest") + .def(py::init(), + py::arg("lane_index"), + py::arg("speed_limit")); + // EASY TRAFFIC LIGHT HANDLE =============================================== py::class_(m, "Waypoint") .def(py::init Date: Tue, 19 Mar 2024 23:00:27 +0800 Subject: [PATCH 041/119] Fix interaction between emergency pullover and finishing task (#333) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/TaskManager.cpp | 70 ++++++++++++------- .../src/rmf_fleet_adapter/TaskManager.hpp | 4 ++ 2 files changed, 50 insertions(+), 24 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 67d9cc15e..c15c7f382 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -93,30 +93,8 @@ TaskManagerPtr TaskManager::make( [w = self->weak_from_this()](const auto&) { const auto self = w.lock(); - - if (!self->_emergency_active) - return; - - auto task_id = "emergency_pullover." + self->_context->name() + "." - + self->_context->group() + "-" - + std::to_string(self->_count_emergency_pullover++); - self->_context->current_task_id(task_id); - - // TODO(MXG): Consider subscribing to the emergency pullover update - self->_emergency_pullover = ActiveTask::start( - events::EmergencyPullover::start( - task_id, - self->_context, - self->_update_cb(), - self->_make_resume_from_emergency()), - self->_context->now()); - - self->_context->worker().schedule( - [w = self->weak_from_this()](const auto&) - { - if (const auto self = w.lock()) - self->_process_robot_interrupts(); - }); + if (self && self->_emergency_active) + self->_begin_pullover(); }); }; @@ -1279,17 +1257,24 @@ bool TaskManager::kill_task( void TaskManager::_begin_next_task() { if (_active_task) + { return; + } if (_emergency_active) + { return; + } std::lock_guard guard(_mutex); if (_queue.empty() && _direct_queue.empty()) { + if (!_waiting && !_finished_waiting) + { _begin_waiting(); + } return; } @@ -1393,6 +1378,33 @@ void TaskManager::_begin_next_task() }); } +//============================================================================== +void TaskManager::_begin_pullover() +{ + _finished_waiting = false; + auto task_id = "emergency_pullover." + _context->name() + "." + + _context->group() + "-" + + std::to_string(_count_emergency_pullover++); + _context->current_task_id(task_id); + + // TODO(MXG): Consider subscribing to the emergency pullover update + _emergency_pullover = ActiveTask::start( + events::EmergencyPullover::start( + task_id, + _context, + _update_cb(), + _make_resume_from_emergency()), + _context->now()); + + _context->worker().schedule( + [w = weak_from_this()](const auto&) + { + if (const auto self = w.lock()) + self->_process_robot_interrupts(); + }); + +} + //============================================================================== void TaskManager::_process_robot_interrupts() { @@ -1565,12 +1577,17 @@ void TaskManager::_resume_from_emergency() return; if (self->_emergency_active) + { return; + } self->_emergency_pullover = ActiveTask(); if (!self->_emergency_pullover_interrupt_token.has_value()) + { + self->_begin_next_task(); return; + } if (self->_active_task) { @@ -1606,6 +1623,11 @@ std::function TaskManager::_make_resume_from_waiting() self->_finished_waiting = true; self->_waiting = ActiveTask(); self->_begin_next_task(); + + if (self->_emergency_active) + { + self->_begin_pullover(); + } }); }; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 52de08f81..575da4697 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -382,6 +382,10 @@ class TaskManager : public std::enable_shared_from_this /// Callback for task timer which begins next task if its deployment time has passed void _begin_next_task(); + /// Begin performing an emergency pullover. This should only be called when an + /// emergency is active. + void _begin_pullover(); + // Interrupts that were issued when there was no active task. They will be // applied when a task becomes active. // From e92d1658188f35cb70f09beb92486b31cbd437dc Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 21 Mar 2024 12:17:19 +0800 Subject: [PATCH 042/119] Support labels in booking information (#328) Signed-off-by: Aaron Chong Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/TaskManager.cpp | 7 ++++++- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 11 +++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index c15c7f382..8771e98c7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -414,7 +414,12 @@ void copy_booking_data( booking_json["unix_millis_request_time"] = to_millis(request_time.value().time_since_epoch()).count(); } - // TODO(MXG): Add priority and labels + const auto labels = booking.labels(); + if (labels.size() != 0) + { + booking_json["labels"] = booking.labels(); + } + // TODO(MXG): Add priority } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 5a24d0bfc..652f35024 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -283,6 +283,11 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( requester = i_it->get(); } + std::vector labels = {}; + const auto labels_it = request_msg.find("labels"); + if (labels_it != request_msg.end()) + labels = labels_it->get>(); + rmf_task::Task::ConstBookingPtr booking = requester.has_value() ? std::make_shared( task_id, @@ -290,12 +295,14 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( priority, requester.value(), request_time, - false) : + false, + labels) : std::make_shared( task_id, earliest_start_time, priority, - false); + false, + labels); const auto new_request = std::make_shared( std::move(booking), deserialized_task.description); From 07b4a524c798ec7eb13088b8c326a612b1000a5a Mon Sep 17 00:00:00 2001 From: Xiyu Date: Mon, 25 Mar 2024 13:22:39 +0800 Subject: [PATCH 043/119] Release other mutexes if robot started charging (#334) Signed-off-by: Xiyu Oh Signed-off-by: Arjo Chakravarty --- .../phases/WaitForCharge.cpp | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index cd48937c2..1b397e1cd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -94,6 +94,30 @@ WaitForCharge::Active::Active( .get_observable() .start_with(initial_msg); + // If the charging waypoint has a mutex group, release all other mutexes + std::unordered_set retain_mutexes; + const auto charging_waypoint = _context->dedicated_charging_wp(); + const auto& graph = _context->navigation_graph(); + retain_mutexes.insert( + graph.get_waypoint(charging_waypoint).in_mutex_group()); + _context->retain_mutex_groups(retain_mutexes); + + if (retain_mutexes.begin()->empty()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is waiting to charge. All its mutex groups will be released.", + _context->name().c_str()); + } + else + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is waiting to charge. It will retain only the mutex group [%s].", + _context->name().c_str(), + retain_mutexes.begin()->c_str()); + } + _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_CHARGING); } From fac1703cb110be930bdd75f39f6c39f0d24af78c Mon Sep 17 00:00:00 2001 From: cwrx777 Date: Mon, 25 Mar 2024 13:23:21 +0800 Subject: [PATCH 044/119] Add all_known_lifts in Graph binding (#336) Signed-off-by: Charly Wu Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter_python/src/graph/graph.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index 0aaf60172..ce17b8edd 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -195,7 +195,10 @@ void bind_graph(py::module& m) .def_property_readonly("num_lanes", &Graph::num_lanes) .def("lanes_from_waypoint", py::overload_cast(&Graph::lanes_from, py::const_), - py::arg("wp_index")); + py::arg("wp_index")) + + //Lifts + .def_property_readonly("all_known_lifts", &Graph::all_known_lifts); // PARSE GRAPH ============================================================== // Helper function to parse a graph from a yaml file From 8a4b9e8a46ec947af0fc54c0853bd26fcd235878 Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 8 Apr 2024 15:13:23 +0800 Subject: [PATCH 045/119] Stabilize commissioning feature (#338) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../agv/RobotUpdateHandle.hpp | 61 +++ .../src/rmf_fleet_adapter/TaskManager.cpp | 384 ++++++++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 37 +- .../agv/FleetUpdateHandle.cpp | 425 +++++++++++++----- .../rmf_fleet_adapter/agv/RobotContext.cpp | 15 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 20 +- .../agv/RobotUpdateHandle.cpp | 120 ++++- .../agv/internal_FleetUpdateHandle.hpp | 42 +- .../agv/internal_RobotUpdateHandle.hpp | 26 ++ rmf_fleet_adapter_python/src/adapter.cpp | 26 +- 10 files changed, 970 insertions(+), 186 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 22da7b444..1a7514c34 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -411,6 +411,64 @@ class RobotUpdateHandle /// If the robot is holding onto a session with a lift, release that session. void release_lift(); + /// A description of whether the robot should accept dispatched and/or direct + /// tasks. + class Commission + { + public: + /// Construct a Commission description with all default values. + /// - accept_dispatched_tasks: true + /// - accept_direct_tasks: true + /// - is_performing_idle_behavior: true + Commission(); + + /// Construct a Commission description that accepts no tasks at all. + /// - accept_dispatch_tasks: false + /// - accept_direct_tasks: false + /// - is_performing_idle_behavior: false + static Commission decommission(); + + /// Set whether this commission should accept dispatched tasks. + Commission& accept_dispatched_tasks(bool decision = true); + + /// Check whether this commission is accepting dispatched tasks. + bool is_accepting_dispatched_tasks() const; + + /// Set whether this commission should accept direct tasks + Commission& accept_direct_tasks(bool decision = true); + + /// Check whether this commission is accepting direct tasks. + bool is_accepting_direct_tasks() const; + + /// Set whether this commission should perform idle behaviors (formerly + /// referred to as "finishing tasks"). + Commission& perform_idle_behavior(bool decision = true); + + /// Check whether this commission is performing idle behaviors (formerly + /// referred to as "finishing tasks"). + bool is_performing_idle_behavior() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + /// Set the current commission for the robot. + void set_commission(Commission commission); + + /// Get the current commission for the robot. If the robot has been dropped + /// from the fleet, this will return Commission::decommission(). + Commission commission() const; + + /// Tell the fleet adapter to reassign all the tasks that have been dispatched + /// to this robot. To prevent the tasks from being reassigned back to this + /// robot use .set_commission(Commission::decommission()) + /// + /// In the current implementation, tasks will only be reassigned to robots + /// in the same fleet that the task was originally assigned to. This behavior + /// could change in the future. + void reassign_dispatched_tasks(); + class Implementation; /// This API is experimental and will not be supported in the future. Users @@ -420,15 +478,18 @@ class RobotUpdateHandle public: /// True if this robot is allowed to accept new tasks. False if the robot /// will not accept any new tasks. + [[deprecated("Use commission instead")]] bool is_commissioned() const; /// Stop this robot from accepting any new tasks. It will continue to /// perform tasks that are already in its queue. To reassign those tasks, /// you will need to use the task request API to cancel the tasks and /// re-request them. + [[deprecated("Use set_commission instead")]] void decommission(); /// Allow this robot to resume accepting new tasks. + [[deprecated("Use set_commission instead")]] void recommission(); /// Get the schedule participant of this robot diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 8771e98c7..78a50f5c3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -64,6 +64,9 @@ #include #include #include +#include +#include +#include namespace rmf_fleet_adapter { @@ -225,7 +228,10 @@ TaskManagerPtr TaskManager::make( rmf_api_msgs::schemas::skip_phase_response, rmf_api_msgs::schemas::task_request, rmf_api_msgs::schemas::undo_skip_phase_request, - rmf_api_msgs::schemas::undo_skip_phase_response + rmf_api_msgs::schemas::undo_skip_phase_response, + rmf_api_msgs::schemas::commission, + rmf_api_msgs::schemas::robot_commission_request, + rmf_api_msgs::schemas::robot_commission_response, }; for (const auto& schema : schemas) @@ -807,17 +813,7 @@ bool TaskManager::cancel_task_if_present(const std::string& task_id) return true; } - std::lock_guard lock(_mutex); - for (auto it = _queue.begin(); it != _queue.end(); ++it) - { - if (it->request()->booking()->id() == task_id) - { - _queue.erase(it); - return true; - } - } - - return false; + return _cancel_task_from_dispatch_queue(task_id, {"DispatchRequest"}); } //============================================================================== @@ -836,7 +832,7 @@ std::string TaskManager::robot_status() const //============================================================================== auto TaskManager::expected_finish_state() const -> State { - std::lock_guard lock(_mutex); + std::lock_guard lock(_mutex); if (!_direct_queue.empty()) { return _direct_queue.rbegin()->assignment.finish_state(); @@ -886,7 +882,7 @@ void TaskManager::enable_responsive_wait(bool value) if (_responsive_wait_enabled) { - std::lock_guard guard(_mutex); + std::lock_guard guard(_mutex); if (!_active_task && _queue.empty() && _direct_queue.empty() && !_waiting) { _begin_waiting(); @@ -901,7 +897,7 @@ void TaskManager::set_idle_task(rmf_task::ConstRequestFactoryPtr task) return; _idle_task = std::move(task); - std::lock_guard guard(_mutex); + std::lock_guard guard(_mutex); if (!_active_task && _queue.empty() && _direct_queue.empty()) { _begin_waiting(); @@ -915,7 +911,7 @@ void TaskManager::set_queue( // We indent this block as _mutex is also locked in the _begin_next_task() // function that is called at the end of this function. { - std::lock_guard guard(_mutex); + std::lock_guard guard(_mutex); // Do not remove automatic task if assignments is empty. See Issue #138 if (assignments.empty() && _queue.size() == 1 && @@ -954,12 +950,12 @@ void TaskManager::set_queue( } //============================================================================== -const std::vector TaskManager::requests() const +std::vector TaskManager::dispatched_requests() const { using namespace rmf_task::requests; std::vector requests; + std::lock_guard lock(_mutex); requests.reserve(_queue.size()); - std::lock_guard lock(_mutex); for (const auto& task : _queue) { if (task.request()->booking()->automatic()) @@ -972,6 +968,107 @@ const std::vector TaskManager::requests() const return requests; } +//============================================================================== +void TaskManager::reassign_dispatched_requests( + std::function on_success, + std::function)> on_failure) +{ + std::vector assignments = _drain_dispatched_assignments(); + const auto fleet = _fleet_handle.lock(); + if (!fleet) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Attempting to reassign tasks for [%s] but its fleet is shutting down", + _context->requester_id().c_str()); + return; + } + + auto& fleet_impl = agv::FleetUpdateHandle::Implementation::get(*fleet); + auto& unassigned = fleet_impl.unassigned_requests; + for (const auto& a : assignments) + { + unassigned.push_back(a.request()); + } + + fleet_impl.reassign_dispatched_tasks( + [name = _context->requester_id(), node = _context->node(), on_success]() + { + RCLCPP_INFO( + node->get_logger(), + "Successfully reassigned tasks for [%s]", + name.c_str()); + on_success(); + }, + [ + name = _context->requester_id(), + node = _context->node(), + assignments, + fleet, + on_failure, + self = shared_from_this() + ](std::vector errors) + { + std::stringstream ss_errors; + if (errors.empty()) + { + ss_errors << " no reason given"; + } + else + { + for (auto&& e : errors) + { + ss_errors << "\n -- " << e; + } + } + + std::stringstream ss_requests; + if (assignments.empty()) + { + ss_requests << "No tasks were assigned to the robot."; + } + else + { + ss_requests << "The following tasks will be canceled:"; + for (const auto& a : assignments) + { + ss_requests << "\n -- " << a.request()->booking()->id(); + } + } + + RCLCPP_ERROR( + node->get_logger(), + "Failed to reassign tasks for [%s]. %s\nReasons for failure:%s", + ss_requests.str().c_str(), + ss_errors.str().c_str()); + + auto& fleet_impl = agv::FleetUpdateHandle::Implementation::get(*fleet); + auto& unassigned = fleet_impl.unassigned_requests; + const auto r_it = std::remove_if( + unassigned.begin(), + unassigned.end(), + [&assignments](const auto& r) + { + return std::find_if( + assignments.begin(), + assignments.end(), + [r](const auto& a) + { + return a.request() == r; + }) != assignments.end(); + }); + unassigned.erase(r_it, unassigned.end()); + + for (const auto& a : assignments) + { + self->_publish_canceled_pending_task(a, {"Failure to reassign"}); + self->_register_executed_task(a.request()->booking()->id()); + } + + on_failure(errors); + }); +} + //============================================================================== TaskManager::RobotModeMsg TaskManager::robot_mode() const { @@ -1023,6 +1120,13 @@ nlohmann::json TaskManager::submit_direct_request( const auto& fleet = _context->group(); const auto& robot = _context->name(); + if (!_context->commission().is_accepting_direct_tasks()) + { + return _make_error_response( + 20, "Uncommissioned", "The robot [" + robot + "] in fleet [" + + fleet + "] is not commissioned to perform direct tasks."); + } + const auto& impl = agv::FleetUpdateHandle::Implementation::get(*fleet_handle); std::vector errors; @@ -1048,7 +1152,11 @@ nlohmann::json TaskManager::submit_direct_request( } catch (const std::exception&) { - json_errors.push_back(e); + nlohmann::json error; + error["code"] = 42; + error["category"] = "unknown"; + error["detail"] = e; + json_errors.push_back(error); } } response_json["errors"] = std::move(json_errors); @@ -1110,7 +1218,7 @@ nlohmann::json TaskManager::submit_direct_request( }; ++_next_sequence_number; { - std::lock_guard lock(_mutex); + std::lock_guard lock(_mutex); _direct_queue.insert(assignment); } @@ -1226,7 +1334,7 @@ bool TaskManager::cancel_task( // TODO(YV): We could cache the task_ids of direct and dispatched tasks in // unordered_sets and perform a lookup to see which function to call. - std::lock_guard lock(_mutex); + std::lock_guard lock(_mutex); if (_cancel_task_from_dispatch_queue(task_id, labels)) return true; @@ -1248,7 +1356,7 @@ bool TaskManager::kill_task( return true; } - std::lock_guard lock(_mutex); + std::lock_guard lock(_mutex); if (_cancel_task_from_dispatch_queue(task_id, labels)) return true; @@ -1271,7 +1379,7 @@ void TaskManager::_begin_next_task() return; } - std::lock_guard guard(_mutex); + std::lock_guard guard(_mutex); if (_queue.empty() && _direct_queue.empty()) { @@ -1495,6 +1603,13 @@ std::function TaskManager::_robot_interruption_callback() //============================================================================== void TaskManager::_begin_waiting() { + if (!_context->commission().is_performing_idle_behavior()) + { + // This robot is not supposed to perform its idle behavior, so we + // immediately from here. + return; + } + if (_idle_task) { const auto request = _idle_task->make_request(_context->make_get_state()()); @@ -1644,7 +1759,7 @@ void TaskManager::retreat_to_charger() return; { - std::lock_guard guard(_mutex); + std::lock_guard guard(_mutex); if (_active_task || !_queue.empty()) return; } @@ -1717,7 +1832,7 @@ void TaskManager::retreat_to_charger() charging_assignment}; ++_next_sequence_number; { - std::lock_guard lock(_mutex); + std::lock_guard lock(_mutex); _direct_queue.insert(assignment); } @@ -1899,6 +2014,8 @@ rmf_task::State TaskManager::_publish_pending_task( { const auto info = pending.request()->description()->generate_info( std::move(expected_state), parameters); + PendingInfo cache; + cache.info = info; nlohmann::json pending_json; const auto& booking = *pending.request()->booking(); @@ -1912,13 +2029,18 @@ rmf_task::State TaskManager::_publish_pending_task( if (pending.finish_state().time()) { - pending_json["unix_millis_finish_time"] = + PendingTimeInfo t; + t.unix_millis_finish_time = to_millis(pending.finish_state().time()->time_since_epoch()).count(); const auto estimate = pending.finish_state().time().value() - pending.deployment_time(); - pending_json["original_estimate_millis"] = - std::max(0l, to_millis(estimate).count()); + t.original_estimate_millis = std::max(0l, to_millis(estimate).count()); + + pending_json["unix_millis_finish_time"] = t.unix_millis_finish_time; + pending_json["original_estimate_millis"] = t.original_estimate_millis; + + cache.time = t; } copy_assignment(pending_json["assigned_to"], *_context); pending_json["status"] = "queued"; @@ -1931,6 +2053,7 @@ rmf_task::State TaskManager::_publish_pending_task( _validate_and_publish_websocket(task_state_update, validator); + _pending_task_info[pending.request()] = cache; return pending.finish_state(); } @@ -1939,6 +2062,7 @@ void TaskManager::_publish_task_queue() { rmf_task::State expected_state = _context->current_task_end_state(); const auto& parameters = *_context->task_parameters(); + _pending_task_info.clear(); for (const auto& pending : _direct_queue) { @@ -1962,6 +2086,20 @@ void TaskManager::_publish_canceled_pending_task( const auto& booking = *pending.request()->booking(); copy_booking_data(pending_json["booking"], booking); + const auto info_it = _pending_task_info.find(pending.request()); + if (info_it != _pending_task_info.end()) + { + const auto& cache = info_it->second; + pending_json["category"] = cache.info.category; + pending_json["detail"] = cache.info.detail; + if (cache.time.has_value()) + { + const auto& t = *cache.time; + pending_json["unix_millis_finish_time"] = t.unix_millis_finish_time; + pending_json["original_estimate_millis"] = t.original_estimate_millis; + } + } + pending_json["unix_millis_start_time"] = to_millis(pending.deployment_time().time_since_epoch()).count(); @@ -1983,17 +2121,59 @@ void TaskManager::_publish_canceled_pending_task( _validate_and_publish_websocket(task_state_update, validator); } +//============================================================================== +auto TaskManager::_drain_dispatched_assignments() -> std::vector +{ + std::vector assignments; + std::lock_guard lock(_mutex); + for (const auto& a : _queue) + { + if (a.request()->booking()->automatic()) + { + continue; + } + + assignments.push_back(a); + } + _queue.clear(); + + return assignments; +} + +//============================================================================== +auto TaskManager::_drain_direct_assignments() -> std::vector +{ + std::vector assignments; + std::lock_guard lock(_mutex); + for (const auto& a : _direct_queue) + { + if (a.assignment.request()->booking()->automatic()) + { + continue; + } + + assignments.push_back(a.assignment); + } + _direct_queue.clear(); + + return assignments; +} + //============================================================================== bool TaskManager::_cancel_task_from_dispatch_queue( const std::string& task_id, const std::vector& labels) { + std::lock_guard lock(_mutex); for (auto it = _queue.begin(); it != _queue.end(); ++it) { if (it->request()->booking()->id() == task_id) { _publish_canceled_pending_task(*it, labels); _queue.erase(it); + + // Count this as an executed task so we don't lose track of its existence + _register_executed_task(task_id); return true; } } @@ -2119,7 +2299,7 @@ void TaskManager::_send_simple_error_if_queued( { // TODO(YV): We could cache the task_ids of direct and dispatched tasks in // unordered_sets and perform a lookup to see which queue to iterate. - std::lock_guard lock(_mutex); + std::lock_guard lock(_mutex); for (const auto& a : _queue) { if (a.request()->booking()->id() == task_id) @@ -2275,6 +2455,8 @@ void TaskManager::_handle_request( _handle_undo_skip_phase_request(request_json, request_id); else if (type_str == "robot_task_request") _handle_direct_request(request_json, request_id); + else if (type_str == "robot_commission_request") + _handle_commission_request(request_json, request_id); else return; } @@ -2300,12 +2482,12 @@ void TaskManager::_handle_direct_request( if (!_validate_request_message(request_json, request_validator, request_id)) return; - const auto& robot = request_json["robot"].get(); - if (robot.empty() || robot != _context->name()) + const auto& fleet = request_json["fleet"].get(); + if (fleet != _context->group()) return; - const auto& fleet = request_json["fleet"].get(); - if (fleet.empty() || fleet != _context->group()) + const auto& robot = request_json["robot"].get(); + if (robot != _context->name()) return; const nlohmann::json& request = request_json["request"]; @@ -2313,6 +2495,142 @@ void TaskManager::_handle_direct_request( _validate_and_publish_api_response(response, response_validator, request_id); } +//============================================================================== +namespace { +nlohmann::json simple_success_json() +{ + nlohmann::json successful_result; + successful_result["success"] = true; + return successful_result; +} +} // anonymous namespace + +//============================================================================== +void TaskManager::_handle_commission_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::robot_commission_request); + + static const auto response_validator = + std::make_shared( + _make_validator(rmf_api_msgs::schemas::robot_commission_response)); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& fleet = request_json["fleet"].get(); + if (fleet != _context->group()) + return; + + const auto& robot = request_json["robot"].get(); + if (robot != _context->name()) + return; + + agv::RobotUpdateHandle::Commission commission = _context->commission(); + const nlohmann::json& commission_json = request_json["commission"]; + const auto dispatch_it = commission_json.find("dispatch_tasks"); + if (dispatch_it != commission_json.end()) + { + commission.accept_dispatched_tasks(dispatch_it->get()); + } + + const auto direct_it = commission_json.find("direct_tasks"); + if (direct_it != commission_json.end()) + { + commission.accept_direct_tasks(direct_it->get()); + } + + const auto idle_it = commission_json.find("idle_behavior"); + if (idle_it != commission_json.end()) + { + commission.perform_idle_behavior(idle_it->get()); + } + + _context->set_commission(commission); + + nlohmann::json response_json; + response_json["commission"] = simple_success_json(); + + const auto direct_policy_it = + request_json.find("pending_direct_tasks_policy"); + if (direct_policy_it != request_json.end() + && direct_policy_it->get() == "cancel") + { + const auto assignments = _drain_direct_assignments(); + for (const auto& a : assignments) + { + _publish_canceled_pending_task( + a, {"Canceled by robot commission request [" + request_id + "]"}); + _register_executed_task(a.request()->booking()->id()); + } + } + + response_json["pending_direct_tasks_policy"] = simple_success_json(); + + const auto dispatch_policy_it = + request_json.find("pending_dispatch_tasks_policy"); + if (dispatch_policy_it == request_json.end() + || dispatch_policy_it->get() == "reassign") + { + reassign_dispatched_requests( + [ + request_id, + response_validator = response_validator, + base_response_json = response_json, + self = shared_from_this() + ]() + { + nlohmann::json response_json = base_response_json; + response_json["pending_dispatch_tasks_policy"] = simple_success_json(); + self->_validate_and_publish_api_response( + response_json, *response_validator, request_id); + }, + [ + request_id, + response_validator = response_validator, + base_response_json = response_json, + self = shared_from_this() + ](std::vector errors) + { + nlohmann::json response_json = base_response_json; + nlohmann::json dispatch_json; + dispatch_json["success"] = false; + std::vector errors_json; + for (const auto& e : errors) + { + nlohmann::json error; + error["code"] = 21; + error["category"] = "planner"; + error["detail"] = e; + errors_json.push_back(error); + } + dispatch_json["errors"] = errors_json; + response_json["pending_dispatch_tasks_policy"] = dispatch_json; + self->_validate_and_publish_api_response( + response_json, *response_validator, request_id); + }); + } + else + { + if (dispatch_policy_it->get() == "cancel") + { + std::vector assignments = _drain_dispatched_assignments(); + for (const auto& a : assignments) + { + _publish_canceled_pending_task( + a, {"Canceled by robot commission request [" + request_id + "]"}); + _register_executed_task(a.request()->booking()->id()); + } + } + + response_json["pending_dispatch_tasks_policy"] = simple_success_json(); + _validate_and_publish_api_response( + response_json, *response_validator, request_id); + } +} + //============================================================================== void TaskManager::_handle_cancel_request( const nlohmann::json& request_json, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 575da4697..7a409a7e7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -40,6 +40,18 @@ namespace rmf_fleet_adapter { +struct PendingTimeInfo +{ + int64_t unix_millis_finish_time; + int64_t original_estimate_millis; +}; + +struct PendingInfo +{ + rmf_task::Task::Description::Info info; + std::optional time; +}; + //============================================================================== /// This task manager is a first attempt at managing multiple tasks per fleet. /// This is a simple implementation that only makes a modest attempt at being @@ -118,7 +130,13 @@ class TaskManager : public std::enable_shared_from_this void set_queue(const std::vector& assignments); /// Get the non-charging requests among pending tasks - const std::vector requests() const; + std::vector dispatched_requests() const; + + /// Send all dispatched requests that are still in the queue back to the fleet + /// handle for reassignment. + void reassign_dispatched_requests( + std::function on_success, + std::function)> on_failure); std::optional current_task_id() const; @@ -320,6 +338,11 @@ class TaskManager : public std::enable_shared_from_this uint16_t _count_emergency_pullover = 0; // Queue for dispatched tasks std::vector _queue; + std::unordered_map< + rmf_task::ConstRequestPtr, + PendingInfo + > _pending_task_info; + // An ID to keep track of the FIFO order of direct tasks std::size_t _next_sequence_number; // Queue for directly assigned tasks @@ -338,7 +361,7 @@ class TaskManager : public std::enable_shared_from_this // TODO: Eliminate the need for a mutex by redesigning the use of the task // manager so that modifications of shared data only happen on designated // rxcpp worker - mutable std::mutex _mutex; + mutable std::recursive_mutex _mutex; rclcpp::TimerBase::SharedPtr _task_timer; rclcpp::TimerBase::SharedPtr _retreat_timer; rclcpp::TimerBase::SharedPtr _update_timer; @@ -437,6 +460,12 @@ class TaskManager : public std::enable_shared_from_this const Assignment& assignment, std::vector labels); + /// Take all dispatched assignments out of the queue, leaving the queue empty. + std::vector _drain_dispatched_assignments(); + + /// Take all direct assignments out of the queue, leaving the queue empty. + std::vector _drain_direct_assignments(); + /// Cancel a task that is in the dispatch queue. Returns false if the task /// was not present. bool _cancel_task_from_dispatch_queue( @@ -539,6 +568,10 @@ class TaskManager : public std::enable_shared_from_this const nlohmann::json& request_json, const std::string& request_id); + void _handle_commission_request( + const nlohmann::json& request_json, + const std::string& request_id); + void _handle_cancel_request( const nlohmann::json& request_json, const std::string& request_id); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 652f35024..2afd86eee 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -340,10 +340,18 @@ class AllocateTasks : public std::enable_shared_from_this expect.states.size(), expect.pending_requests.size()); + std::unordered_map robot_indexes; + std::vector states; + for (const auto& [context, state] : expect.states) + { + robot_indexes[states.size()] = context; + states.push_back(state); + } + // Generate new task assignments const auto result = task_planner.plan( rmf_traffic_ros2::convert(node->now()), - expect.states, + states, expect.pending_requests); auto assignments_ptr = std::get_if< @@ -394,7 +402,22 @@ class AllocateTasks : public std::enable_shared_from_this return std::nullopt; } - const auto assignments = *assignments_ptr; + TaskAssignments assignments; + for (std::size_t i = 0; i < assignments_ptr->size(); ++i) + { + const auto r_it = robot_indexes.find(i); + if (r_it == robot_indexes.end()) + { + RCLCPP_ERROR( + node->get_logger(), + "[AllocateTasks] Unable to find a robot associated with index [%d]", + i); + continue; + } + + const auto context = r_it->second; + assignments[context] = (*assignments_ptr)[i]; + } if (assignments.empty()) { @@ -541,15 +564,15 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const auto& assignments = allocation_result.value(); - const double cost = self->_pimpl->task_planner->compute_cost(assignments); + const double cost = self->_pimpl->compute_cost(assignments); // Display computed assignments for debugging std::stringstream debug_stream; debug_stream << "Cost: " << cost << std::endl; - for (std::size_t i = 0; i < assignments.size(); ++i) + for (const auto& [context, queue] : assignments) { - debug_stream << "--Agent: " << i << std::endl; - for (const auto& a : assignments[i]) + debug_stream << "--Agent: " << context->requester_id() << std::endl; + for (const auto& a : queue) { const auto& s = a.finish_state(); const double request_seconds = @@ -576,31 +599,19 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( "%s", debug_stream.str().c_str()); - // Map robot index to name to populate robot_name in BidProposal - std::unordered_map robot_name_map; - std::size_t index = 0; - for (const auto& t : self->_pimpl->task_managers) - { - robot_name_map.insert({index, t.first->name()}); - ++index; - } - std::optional robot_name; std::optional finish_time; - index = 0; - for (const auto& agent : assignments) + for (const auto& [context, queue] : assignments) { - for (const auto& assignment : agent) + for (const auto& assignment : queue) { if (assignment.request()->booking()->id() == task_id) { finish_time = assignment.finish_state().time().value(); - if (robot_name_map.find(index) != robot_name_map.end()) - robot_name = robot_name_map[index]; + robot_name = context->name(); break; } } - ++index; } if (!robot_name.has_value() || !finish_time.has_value()) @@ -660,6 +671,7 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( dispatch_ack.dispatch_id = msg->dispatch_id; if (msg->type == DispatchCmdMsg::TYPE_AWARD) { + last_bid_assignment = task_id; const auto task_it = bid_notice_assignments.find(task_id); if (task_it == bid_notice_assignments.end()) { @@ -715,23 +727,6 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( auto assignments = std::move(task_it->second); bid_notice_assignments.erase(task_it); - if (assignments.size() != task_managers.size()) - { - // FIXME(MXG): This error mode seems like a problem with our - // implementation. If a robot is added during a bid process, we could - // trigger this error even though it shouldn't actually be a problem. - - std::string error_str = - "The number of available robots does not match that in the assignments " - "for task_id [" + task_id + "]. This request will be ignored."; - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - dispatch_ack.errors.push_back( - make_error_str(13, "Internal bug", std::move(error_str))); - - dispatch_ack_pub->publish(dispatch_ack); - return; - } - // Here we make sure none of the tasks in the assignments has already begun // execution. If so, we replan assignments until a valid set is obtained // and only then update the task manager queues @@ -739,9 +734,9 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( if (!valid_assignments) { rmf_task::ConstRequestPtr request; - for (const auto& a : assignments) + for (const auto& [_, queue] : assignments) { - for (const auto& r : a) + for (const auto& r : queue) { if (r.request()->booking()->id() == task_id) { @@ -768,42 +763,71 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( return; } - // TODO: This replanning is blocking the main thread. Instead, the - // replanning should run on a separate worker and then deliver the - // result back to the main worker. - const auto replan_results = AllocateTasks( - nullptr, aggregate_expectations(), *task_planner, node) - .run(dispatch_ack.errors); + unassigned_requests.push_back(request); + reassign_dispatched_tasks( + // On success + [dispatch_ack, dispatch_ack_pub = dispatch_ack_pub]() + { + auto msg = dispatch_ack; + msg.success = true; + dispatch_ack_pub->publish(dispatch_ack); + }, + // On failure + [dispatch_ack, w = weak_self, request]( + std::vector errors) + { + const auto self = w.lock(); + if (!self) + return; - if (!replan_results) - { - std::string error_str = - "Unable to replan assignments when accommodating task_id [" + task_id - + "]. This request will be ignored."; + auto r_it = std::remove( + self->_pimpl->unassigned_requests.begin(), + self->_pimpl->unassigned_requests.end(), + request); - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - dispatch_ack.errors.push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - dispatch_ack_pub->publish(dispatch_ack); - return; - } + self->_pimpl->unassigned_requests.erase( + r_it, self->_pimpl->unassigned_requests.end()); + + std::string error_str = + "Unable to replan assignments when accommodating task_id [" + + request->booking()->id() + "]. Reasons:"; + if (errors.empty()) + { + error_str += " No reason given by planner."; + } + else + { + for (const auto& e : errors) + { + error_str += "\n -- " + e; + } + } - assignments = replan_results.value(); - // We do not need to re-check if assignments are valid as this function - // is being called by the ROS2 executor and is running on the main - // rxcpp worker. Hence, no new tasks would have started during this - // replanning. + RCLCPP_ERROR( + self->_pimpl->node->get_logger(), + "%s", + error_str.c_str()); + auto msg = dispatch_ack; + msg.success = false; + msg.errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + self->_pimpl->dispatch_ack_pub->publish(msg); + }); } else { - std::size_t index = 0; - for (auto& t : task_managers) + for (const auto& [context, queue] : assignments) { - t.second->set_queue(assignments[index]); - ++index; + context->task_manager()->set_queue(queue); } - current_assignment_cost = task_planner->compute_cost(assignments); + // Any unassigned requests would have been collected by the aggregator and + // given an assignment while calculating this bid. As long as + // is_valid_assignments was able to pass, then all tasks have been + // assigned to properly commissioned robots. + unassigned_requests.clear(); + + current_assignment_cost = compute_cost(assignments); dispatch_ack.success = true; dispatch_ack_pub->publish(dispatch_ack); @@ -836,52 +860,39 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( if (task_was_found) { - // Re-plan assignments while ignoring request for task to be cancelled - std::vector errors; - const auto replan_results = AllocateTasks( - nullptr, aggregate_expectations(), *task_planner, node) - .run(errors); - - if (!replan_results.has_value()) - { - std::stringstream ss; - ss << "Unabled to replan assignments when cancelling task [" - << task_id << "]. "; - if (errors.empty()) + reassign_dispatched_tasks( + // On success + [task_id, name = name, node = node]() { - ss << "No planner error messages were provided."; - } - else + RCLCPP_INFO( + node->get_logger(), + "Task with task_id [%s] has successfully been cancelled. " + "TaskAssignments updated for robots in fleet [%s].", + task_id.c_str(), name.c_str()); + }, + // On failure + [task_id, name = name, node = node](std::vector errors) { - ss << "The following planner errors occurred:"; - for (const auto& e : errors) + std::stringstream ss; + ss << "Unabled to replan assignments when cancelling task [" + << task_id << "] for fleet [" << name << "]. "; + if (errors.empty()) { - const auto err = nlohmann::json::parse(e); - ss << "\n -- " << err["detail"].get(); + ss << "No planner error messages were provided."; } - } - ss << "\n"; - - RCLCPP_WARN(node->get_logger(), "%s", ss.str().c_str()); - } - else - { - const auto& assignments = replan_results.value(); - std::size_t index = 0; - for (auto& t : task_managers) - { - t.second->set_queue(assignments[index]); - ++index; - } - - current_assignment_cost = task_planner->compute_cost(assignments); + else + { + ss << "The following planner errors occurred:"; + for (const auto& e : errors) + { + const auto err = nlohmann::json::parse(e); + ss << "\n -- " << err["detail"].get(); + } + } + ss << "\n"; - RCLCPP_INFO( - node->get_logger(), - "Task with task_id [%s] has successfully been cancelled. TaskAssignments " - "updated for robots in fleet [%s].", - task_id.c_str(), name.c_str()); - } + RCLCPP_WARN(node->get_logger(), "%s", ss.str().c_str()); + }); } } @@ -899,9 +910,24 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( } } +//============================================================================== +double FleetUpdateHandle::Implementation::compute_cost( + const TaskAssignments& assignments) const +{ + rmf_task::TaskPlanner::Assignments raw_assignments; + raw_assignments.reserve(assignments.size()); + for (const auto [_, queue] : assignments) + { + raw_assignments.push_back(queue); + } + + return task_planner->compute_cost(raw_assignments); +} + //============================================================================== auto FleetUpdateHandle::Implementation::is_valid_assignments( - TaskAssignments& assignments) const -> bool + TaskAssignments& assignments, + std::string* report_error) const -> bool { std::unordered_set executed_tasks; for (const auto& [context, mgr] : task_managers) @@ -910,19 +936,171 @@ auto FleetUpdateHandle::Implementation::is_valid_assignments( executed_tasks.insert(tasks.begin(), tasks.end()); } - for (const auto& agent : assignments) + for (const auto& [context, queue] : assignments) { - for (const auto& a : agent) + for (const auto& a : queue) { if (executed_tasks.find(a.request()->booking()->id()) != executed_tasks.end()) + { + if (report_error) + { + *report_error = "task [" + a.request()->booking()->id() + + "] already began executing"; + } + return false; + } + } + + if (!queue.empty()) + { + if (!context->commission().is_accepting_dispatched_tasks()) + { + if (report_error) + { + *report_error = "robot [" + context->name() + + "] has been decommissioned"; + } + // If any tasks were assigned to this robot after it has been + // decommissioned, then those were invalid assignments. + return false; + } + + if (task_managers.find(context) == task_managers.end()) + { + if (report_error) + { + *report_error = "robot [" + context->name() + + "] has been removed from the fleet"; + } + // This robot is no longer part of the fleet, so we cannot assign + // tasks to it. return false; + } } } return true; } +//============================================================================== +void FleetUpdateHandle::Implementation::reassign_dispatched_tasks( + std::function on_success, + std::function)> on_failure) +{ + auto expectations = aggregate_expectations(); + if (expectations.states.empty() && !expectations.pending_requests.empty()) + { + // If there are no robots that can perform any tasks but there are pending + // requests, then we should immediately assume failure. + worker.schedule( + [on_failure](const auto&) + { + on_failure({"no more robots available"}); + }); + return; + } + + auto on_plan_received = [ + on_success, + on_failure, + w = weak_self, + initial_last_bid_assignment = last_bid_assignment + ](TaskAssignments assignments) + { + const auto self = w.lock(); + if (!self) + return; + + const bool new_task_came_in = + initial_last_bid_assignment != self->_pimpl->last_bid_assignment; + + std::string error; + const bool valid_assignments = + !new_task_came_in + && self->_pimpl->is_valid_assignments(assignments, &error); + + if (!valid_assignments) + { + // We need to replan because some important aspect of the planning + // problem has changed since we originally tried to reassign. + if (new_task_came_in) + { + RCLCPP_WARN( + self->_pimpl->node->get_logger(), + "Redoing task reassignment for fleet [%s] because a new task was " + "dispatched while the reassignment was being calculated.", + self->_pimpl->name.c_str()); + } + else + { + RCLCPP_WARN( + self->_pimpl->node->get_logger(), + "Redoing task reassignment for fleet [%s] because %s.", + self->_pimpl->name.c_str(), + error.c_str()); + } + + self->_pimpl->reassign_dispatched_tasks(on_success, on_failure); + return; + } + + for (const auto& [context, queue] : assignments) + { + context->task_manager()->set_queue(queue); + } + + // All requests should be assigned now, no matter which robot they were + // originally assigned to, so we can clear this buffer. + // + // As long as no new tasks were dispatched to this fleet (verified by the + // new_task_came_in check), any tasks that might have been previously + // unassigned will be accounted for in the current set of assignments. + self->_pimpl->unassigned_requests.clear(); + + self->_pimpl->current_assignment_cost = + self->_pimpl->compute_cost(assignments); + + on_success(); + }; + + reassignment_worker.schedule( + [ + on_plan_received, + on_failure, + expectations = std::move(expectations), + task_planner = *task_planner, + node = node, + worker = worker + ](const auto&) + { + std::vector errors; + const auto replan_results = AllocateTasks( + nullptr, expectations, task_planner, node) + .run(errors); + + if (replan_results.has_value()) + { + worker.schedule( + [ + assignments = std::move(*replan_results), + on_plan_received + ](const auto&) + { + on_plan_received(assignments); + }); + } + else + { + worker.schedule( + [errors = std::move(errors), on_failure](const auto&) + { + on_failure(errors); + }); + } + }); +} + //============================================================================== std::optional FleetUpdateHandle::Implementation:: get_nearest_charger( @@ -1097,6 +1275,16 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const issue_msg["detail"] = issue->detail; issues_msg.push_back(std::move(issue_msg)); } + + const auto& commission = context->commission(); + nlohmann::json commission_json; + commission_json["dispatch_tasks"] = + commission.is_accepting_dispatched_tasks(); + commission_json["direct_tasks"] = commission.is_accepting_direct_tasks(); + commission_json["idle_behavior"] = + commission.is_performing_idle_behavior(); + + json["commission"] = commission_json; } try @@ -1499,15 +1687,24 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const for (const auto& t : task_managers) { // Ignore any robots that are not currently commissioned. - if (!t.first->is_commissioned()) + if (!t.first->commission().is_accepting_dispatched_tasks()) continue; - expect.states.push_back(t.second->expected_finish_state()); - const auto requests = t.second->requests(); + expect.states.push_back( + ExpectedState { + t.first, + t.second->expected_finish_state() + }); + const auto requests = t.second->dispatched_requests(); expect.pending_requests.insert( expect.pending_requests.end(), requests.begin(), requests.end()); } + expect.pending_requests.insert( + expect.pending_requests.end(), + unassigned_requests.begin(), + unassigned_requests.end()); + return expect; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 3f0da0358..5fc732efb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -971,21 +971,24 @@ std::shared_ptr RobotContext::task_manager() } //============================================================================== -bool RobotContext::is_commissioned() const +void RobotContext::set_commission(RobotUpdateHandle::Commission value) { - return _commissioned; + std::lock_guard lock(*_commission_mutex); + _commission = std::move(value); } //============================================================================== -void RobotContext::decommission() +const RobotUpdateHandle::Commission& RobotContext::commission() const { - _commissioned = false; + return _commission; } //============================================================================== -void RobotContext::recommission() +RobotUpdateHandle::Commission RobotContext::copy_commission() const { - _commissioned = true; + std::lock_guard lock(*_commission_mutex); + RobotUpdateHandle::Commission copy = _commission; + return copy; } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index da39cb27c..b20559015 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -54,6 +54,7 @@ class TaskManager; namespace agv { +class FleetUpdateHandle; class RobotContext; using TransformDictionary = std::unordered_map; using SharedPlanner = std::shared_ptr< @@ -636,13 +637,18 @@ class RobotContext /// Get the task manager for this robot, if it exists. std::shared_ptr task_manager(); - /// Return true if this robot is currently commissioned (available to accept - /// new tasks). - bool is_commissioned() const; + /// Set the commission for this robot + void set_commission(RobotUpdateHandle::Commission value); - void decommission(); + /// Get a reference to the robot's commission. + const RobotUpdateHandle::Commission& commission() const; - void recommission(); + /// Lock the commission_mutex and return a copy of the robot's current + /// commission. + RobotUpdateHandle::Commission copy_commission() const; + + /// Reassign the tasks that have been dispatched for this robot + void reassign_dispatched_tasks(); Reporting& reporting(); @@ -858,7 +864,9 @@ class RobotContext RobotUpdateHandle::Unstable::Watchdog _lift_watchdog; rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0); - bool _commissioned = true; + std::unique_ptr _commission_mutex = + std::make_unique(); + RobotUpdateHandle::Commission _commission; bool _emergency = false; EasyFullControl::LocalizationRequest _localize; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 46fbb44f1..68a46eca0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -720,6 +720,100 @@ void RobotUpdateHandle::release_lift() }); } +//============================================================================== +class RobotUpdateHandle::Commission::Implementation +{ +public: + bool is_accepting_dispatched_tasks = true; + bool is_accepting_direct_tasks = true; + bool is_performing_idle_behavior = true; +}; + +//============================================================================== +RobotUpdateHandle::Commission::Commission() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +auto RobotUpdateHandle::Commission::decommission() -> Commission +{ + return Commission() + .accept_dispatched_tasks(false) + .accept_direct_tasks(false) + .perform_idle_behavior(false); +} + +//============================================================================== +auto RobotUpdateHandle::Commission::accept_dispatched_tasks(bool decision) +-> Commission& +{ + _pimpl->is_accepting_dispatched_tasks = decision; + return *this; +} + +//============================================================================== +bool RobotUpdateHandle::Commission::is_accepting_dispatched_tasks() const +{ + return _pimpl->is_accepting_dispatched_tasks; +} + +//============================================================================== +auto RobotUpdateHandle::Commission::accept_direct_tasks(bool decision) +-> Commission& +{ + _pimpl->is_accepting_direct_tasks = decision; + return *this; +} + +//============================================================================== +bool RobotUpdateHandle::Commission::is_accepting_direct_tasks() const +{ + return _pimpl->is_accepting_direct_tasks; +} + +//============================================================================== +auto RobotUpdateHandle::Commission::perform_idle_behavior(bool decision) +-> Commission& +{ + _pimpl->is_performing_idle_behavior = decision; + return *this; +} + +//============================================================================== +bool RobotUpdateHandle::Commission::is_performing_idle_behavior() const +{ + return _pimpl->is_performing_idle_behavior; +} + +//============================================================================== +void RobotUpdateHandle::set_commission(Commission commission) +{ + _pimpl->set_commission(std::move(commission)); +} + +//============================================================================== +auto RobotUpdateHandle::commission() const -> Commission +{ + return _pimpl->commission(); +} + +//============================================================================== +void RobotUpdateHandle::reassign_dispatched_tasks() +{ + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [context](const auto&) + { + const auto mgr = context->task_manager(); + if (mgr) + mgr->reassign_dispatched_requests([]() {}, [](auto) {}); + }); + } +} + //============================================================================== RobotUpdateHandle::RobotUpdateHandle() { @@ -742,7 +836,7 @@ const RobotUpdateHandle::Unstable& RobotUpdateHandle::unstable() const bool RobotUpdateHandle::Unstable::is_commissioned() const { if (const auto context = _pimpl->get_context()) - return context->is_commissioned(); + return context->copy_commission().is_accepting_dispatched_tasks(); return false; } @@ -750,29 +844,17 @@ bool RobotUpdateHandle::Unstable::is_commissioned() const //============================================================================== void RobotUpdateHandle::Unstable::decommission() { - if (const auto context = _pimpl->get_context()) - { - context->worker().schedule( - [w = context->weak_from_this()](const auto&) - { - if (const auto context = w.lock()) - context->decommission(); - }); - } + _pimpl->set_commission( + _pimpl->commission() + .accept_dispatched_tasks(false)); } //============================================================================== void RobotUpdateHandle::Unstable::recommission() { - if (const auto context = _pimpl->get_context()) - { - context->worker().schedule( - [w = context->weak_from_this()](const auto&) - { - if (const auto context = w.lock()) - context->recommission(); - }); - } + _pimpl->set_commission( + _pimpl->commission() + .accept_dispatched_tasks(true)); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 22369f3ff..2e218e882 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -68,6 +68,7 @@ #include #include #include +#include #include @@ -233,16 +234,27 @@ class ParticipantFactoryRos2 : public ParticipantFactory rmf_traffic_ros2::schedule::WriterPtr _writer; }; +//============================================================================== +struct ExpectedState +{ + RobotContextPtr context; + rmf_task::State state; +}; + //============================================================================== struct Expectations { - std::vector states; + std::vector states; std::vector pending_requests; }; //============================================================================== -// Map task id to pair of -using TaskAssignments = rmf_task::TaskPlanner::Assignments; +// Map from the robot instance to its proposed assignment of tasks +using TaskAssignments = std::unordered_map< + RobotContextPtr, + std::vector>; + +// Forward declaration of a task allocation job class AllocateTasks; //============================================================================== @@ -330,6 +342,13 @@ class FleetUpdateHandle::Implementation // Map to store task id with assignments for BidNotice std::unordered_map bid_notice_assignments = {}; + // This is checked before and after the task reassignment procedure to ensure + // that no new task came in from the dispatcher while the reassignment was + // being calculated. + std::string last_bid_assignment; + std::vector unassigned_requests; + rxcpp::schedulers::worker reassignment_worker; + using BidNoticeMsg = rmf_task_msgs::msg::BidNotice; using DispatchCmdMsg = rmf_task_msgs::msg::DispatchCommand; @@ -364,6 +383,9 @@ class FleetUpdateHandle::Implementation handle->_pimpl = rmf_utils::make_unique_impl( Implementation{handle, std::forward(args)...}); + handle->_pimpl->reassignment_worker = + rxcpp::schedulers::make_event_loop().create_worker(); + handle->_pimpl->add_standard_tasks(); handle->_pimpl->emergency_obs = @@ -479,7 +501,8 @@ class FleetUpdateHandle::Implementation rmf_api_msgs::schemas::robot_state, rmf_api_msgs::schemas::location_2D, rmf_api_msgs::schemas::fleet_log, - rmf_api_msgs::schemas::log_entry + rmf_api_msgs::schemas::log_entry, + rmf_api_msgs::schemas::commission, }; for (const auto& schema : schemas) @@ -617,6 +640,8 @@ class FleetUpdateHandle::Implementation void dispatch_command_cb(const DispatchCmdMsg::SharedPtr msg); + double compute_cost(const TaskAssignments& assignments) const; + std::optional get_nearest_charger( const rmf_traffic::agv::Planner::Start& start); @@ -624,7 +649,14 @@ class FleetUpdateHandle::Implementation /// Helper function to check if assignments are valid. An assignment set is /// invalid if one of the assignments has already begun execution. - bool is_valid_assignments(TaskAssignments& assignments) const; + bool is_valid_assignments( + TaskAssignments& assignments, + std::string* report_error = nullptr) const; + + void reassign_dispatched_tasks( + std::function on_success, + // argument is a vector of error messages from the planner + std::function)> on_failure); void publish_fleet_state_topic() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index b95dc85dc..7e6bc133d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -299,6 +299,32 @@ class RobotUpdateHandle::Implementation return *handle._pimpl; } + void set_commission(Commission commission) + { + if (const auto context = get_context()) + { + context->worker().schedule( + [w = context->weak_from_this(), commission = std::move(commission)]( + const auto&) + { + if (const auto context = w.lock()) + { + context->set_commission(commission); + } + }); + } + } + + Commission commission() const + { + if (const auto context = get_context()) + { + return context->copy_commission(); + } + + return Commission::decommission(); + } + std::shared_ptr get_context(); std::shared_ptr get_context() const; diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 0381dd164..fc1bace85 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -61,6 +61,7 @@ using ActivityIdentifier = agv::RobotUpdateHandle::ActivityIdentifier; using ActionExecution = agv::RobotUpdateHandle::ActionExecution; using RobotInterruption = agv::RobotUpdateHandle::Interruption; using IssueTicket = agv::RobotUpdateHandle::IssueTicket; +using Commission = agv::RobotUpdateHandle::Commission; using Stubbornness = agv::RobotUpdateHandle::Unstable::Stubbornness; void bind_types(py::module&); @@ -291,7 +292,14 @@ PYBIND11_MODULE(rmf_adapter, m) { py::arg("text")) .def("enable_responsive_wait", &agv::RobotUpdateHandle::enable_responsive_wait, - py::arg("value")); + py::arg("value")) + .def("set_commission", + &agv::RobotUpdateHandle::set_commission, + py::arg("commission")) + .def("commission", + &agv::RobotUpdateHandle::commission) + .def("reassign_dispatched_tasks", + &agv::RobotUpdateHandle::reassign_dispatched_tasks); // ACTION EXECUTOR ======================================================= auto m_robot_update_handle = m.def_submodule("robot_update_handle"); @@ -354,6 +362,22 @@ PYBIND11_MODULE(rmf_adapter, m) { .value("Warning", agv::RobotUpdateHandle::Tier::Warning) .value("Error", agv::RobotUpdateHandle::Tier::Error); + // Commission ====================================================== + py::class_( + m_robot_update_handle, "Commission") + .def_property( + "accept_dispatched_tasks", + &Commission::is_accepting_dispatched_tasks, + &Commission::accept_dispatched_tasks) + .def_property( + "accept_direct_tasks", + &Commission::is_accepting_direct_tasks, + &Commission::accept_direct_tasks) + .def_property( + "perform_idle_behavior", + &Commission::is_performing_idle_behavior, + &Commission::perform_idle_behavior); + // Stubbornness ============================================================ py::class_( m_robot_update_handle, "Stubbornness") From fd8fe46ddb0c6bf034537838c80c9796e65238bc Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 8 Apr 2024 16:49:33 +0800 Subject: [PATCH 046/119] Refactors the socket broadcast client (#329) * Refactors the socket broadcast client This commit refactors the socket broadcast client. It switches out the underlying queue to a circular buffer, that way the memory pressure of the broadcast client remains constant without needing to do work at sporadic intervals. The new structure should hopefully also make it easier for someon to reason about the websocket layer. Signed-off-by: Arjo Chakravarty * ROS-ify logging. Signed-off-by: Arjo Chakravarty * Remove unessecary dependency Signed-off-by: Arjo Chakravarty * Add lock Signed-off-by: Arjo Chakravarty * Header guard Signed-off-by: Arjo Chakravarty * Use enum for status Signed-off-by: Arjo Chakravarty * Const reference to prevent copy Signed-off-by: Arjo Chakravarty * Remove redundant include Signed-off-by: Arjo Chakravarty * Fix move Signed-off-by: Arjo Chakravarty * Move everything to one thread. Signed-off-by: Arjo Chakravarty * Fixed "deadlock" Signed-off-by: Arjo Chakravarty * Clean up reconnection logic Signed-off-by: Arjo Chakravarty * Delete move and copy constructors. Signed-off-by: Arjo Chakravarty * Remove FastAPI file Signed-off-by: Arjo Chakravarty * More cleanups Signed-off-by: Arjo Chakravarty * Add unit tests and fix a bunch of corner cases. Signed-off-by: Arjo Chakravarty * Style Signed-off-by: Arjo Chakravarty * Address feedback Signed-off-by: Arjo Chakravarty * Address feedback Signed-off-by: Arjo Chakravarty * Address logging feedback Signed-off-by: Arjo Chakravarty * Address feedback Signed-off-by: Arjo Chakravarty * Style Signed-off-by: Arjo Chakravarty * Horrible hack to make tests always pass. Signed-off-by: Arjo Chakravarty * Address Feedback Tests flaky even after improvements. Signed-off-by: Arjo Chakravarty * Style Signed-off-by: Arjo Chakravarty * Fix tests. Thanks @koonpeng for spotting the timing issue. It seems you were right about the server not being cleanly closed. This commit fixes that by cleanly closing the server in the test. Signed-off-by: Arjo Chakravarty * Address feedback about logging. Signed-off-by: Arjo Chakravarty * Style Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty --- rmf_websocket/CMakeLists.txt | 127 +++++++- rmf_websocket/examples/client.cpp | 53 +++ .../include/rmf_websocket/BroadcastClient.hpp | 5 - .../src/rmf_websocket/BroadcastClient.cpp | 306 +++++++++--------- .../client/ClientWebSocketEndpoint.cpp | 37 ++- .../client/ClientWebSocketEndpoint.hpp | 150 +++++++++ .../src/rmf_websocket/utils/RingBuffer.hpp | 81 +++++ .../rmf_websocket/utils/RingBuffer_TEST.cpp | 36 +++ rmf_websocket/test/test_client.cpp | 115 +++++++ rmf_websocket/test/test_client_no_server.cpp | 21 ++ .../test/test_client_with_update_cb.cpp | 141 ++++++++ 11 files changed, 893 insertions(+), 179 deletions(-) create mode 100644 rmf_websocket/examples/client.cpp create mode 100644 rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.hpp create mode 100644 rmf_websocket/src/rmf_websocket/utils/RingBuffer.hpp create mode 100644 rmf_websocket/src/rmf_websocket/utils/RingBuffer_TEST.cpp create mode 100644 rmf_websocket/test/test_client.cpp create mode 100644 rmf_websocket/test/test_client_no_server.cpp create mode 100644 rmf_websocket/test/test_client_with_update_cb.cpp diff --git a/rmf_websocket/CMakeLists.txt b/rmf_websocket/CMakeLists.txt index abc3bae00..685c553c9 100644 --- a/rmf_websocket/CMakeLists.txt +++ b/rmf_websocket/CMakeLists.txt @@ -21,11 +21,15 @@ find_package(nlohmann_json REQUIRED) find_package(nlohmann_json_schema_validator_vendor REQUIRED) find_package(nlohmann_json_schema_validator REQUIRED) find_package(websocketpp REQUIRED) -find_package(Boost COMPONENTS system filesystem REQUIRED) +find_package(Boost COMPONENTS system REQUIRED) find_package(Threads) -file(GLOB_RECURSE core_lib_srcs "src/rmf_websocket/*.cpp") -add_library(rmf_websocket SHARED ${core_lib_srcs}) + +add_library(rmf_websocket SHARED + src/rmf_websocket/client/ClientWebSocketEndpoint.cpp + src/rmf_websocket/BroadcastClient.cpp + src/rmf_websocket/BroadcastServer.cpp +) target_link_libraries(rmf_websocket PUBLIC @@ -35,8 +39,7 @@ target_link_libraries(rmf_websocket nlohmann_json::nlohmann_json nlohmann_json_schema_validator PRIVATE - ${Boost_FILESYSTEM_LIBRARY} - ${Boost_SYSTEM_LIBRARY} + Boost::system Threads::Threads ) @@ -48,6 +51,26 @@ target_include_directories(rmf_websocket ${WEBSOCKETPP_INCLUDE_DIR} ) +add_executable(example_client + examples/client.cpp) + +target_link_libraries(example_client + PUBLIC + rmf_websocket + ${websocketpp_LIBRARIES} + PRIVATE + Threads::Threads +) + +target_include_directories(example_client + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ${WEBSOCKETPP_INCLUDE_DIR} +) + + ament_export_targets(rmf_websocket HAS_LIBRARY_TARGET) ament_export_dependencies(rmf_traffic rclcpp nlohmann_json websocketpp) @@ -65,4 +88,98 @@ install( ARCHIVE DESTINATION lib ) +if(BUILD_TESTING) + find_package(ament_cmake_uncrustify REQUIRED) + find_file(uncrustify_config_file + NAMES "rmf_code_style.cfg" + PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") + + ament_uncrustify( + include src + CONFIG_FILE ${uncrustify_config_file} + LANGUAGE C++ + MAX_LINE_LENGTH 80 + ) + + # unit test + find_package(ament_cmake_catch2 REQUIRED) + ament_add_catch2(test_ring_buffer + src/rmf_websocket/utils/RingBuffer_TEST.cpp + TIMEOUT 300) + target_link_libraries(test_ring_buffer + PRIVATE + rmf_utils::rmf_utils + Boost::system + ) + +#integration test + find_package(OpenSSL REQUIRED) + ament_add_catch2(test_client + test/test_client.cpp + TIMEOUT 300) + target_link_libraries(test_client + PUBLIC + ${websocketpp_LIBRARIES} + ${rclcpp_LIBRARIES} + rmf_utils::rmf_utils + Boost::system + rmf_websocket + PRIVATE + Threads::Threads + OpenSSL::Crypto + ) + target_include_directories(test_client + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ${WEBSOCKETPP_INCLUDE_DIR} + ) + + + ament_add_catch2(test_client_no_server + test/test_client_no_server.cpp + TIMEOUT 300) + target_link_libraries(test_client_no_server + PUBLIC + ${websocketpp_LIBRARIES} + ${rclcpp_LIBRARIES} + rmf_utils::rmf_utils + Boost::system + rmf_websocket + PRIVATE + Threads::Threads + OpenSSL::Crypto + ) + target_include_directories(test_client_no_server + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ${WEBSOCKETPP_INCLUDE_DIR} + ) + + ament_add_catch2(test_client_with_update_cb + test/test_client_with_update_cb.cpp + TIMEOUT 300) + target_link_libraries(test_client_with_update_cb + PUBLIC + ${websocketpp_LIBRARIES} + ${rclcpp_LIBRARIES} + rmf_utils::rmf_utils + Boost::system + rmf_websocket + PRIVATE + Threads::Threads + OpenSSL::Crypto + ) + target_include_directories(test_client_with_update_cb + PUBLIC + $ + $ + ${rclcpp_INCLUDE_DIRS} + ${WEBSOCKETPP_INCLUDE_DIR} + ) +endif() + ament_package() diff --git a/rmf_websocket/examples/client.cpp b/rmf_websocket/examples/client.cpp new file mode 100644 index 000000000..a41f974c6 --- /dev/null +++ b/rmf_websocket/examples/client.cpp @@ -0,0 +1,53 @@ +#include +#include + +using namespace rmf_websocket; +using namespace std::chrono_literals; + +std::vector new_connection_data() +{ + std::vector msgs; + nlohmann::json json; + json["hi"] = "Hello"; + msgs.push_back(json); + return msgs; +} + +class MinimalPublisher : public rclcpp::Node +{ +public: + MinimalPublisher() + : Node("web_socket_test_node"), count_(0) + { + timer_ = this->create_wall_timer( + 10ms, std::bind(&MinimalPublisher::timer_callback, this)); + } + +private: + void timer_callback() + { + if (!client_) + { + client_ = BroadcastClient::make( + "ws://127.0.0.1:8000/", + shared_from_this(), + &new_connection_data + ); + } + obj["count"] = count_++; + client_->publish(obj); + } + rclcpp::TimerBase::SharedPtr timer_; + nlohmann::json obj {{"Otototo", true}}; + std::size_t count_ = 0; + std::shared_ptr client_ = nullptr; +}; + + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/rmf_websocket/include/rmf_websocket/BroadcastClient.hpp b/rmf_websocket/include/rmf_websocket/BroadcastClient.hpp index 55210d164..4b67ac5bb 100644 --- a/rmf_websocket/include/rmf_websocket/BroadcastClient.hpp +++ b/rmf_websocket/include/rmf_websocket/BroadcastClient.hpp @@ -24,11 +24,6 @@ #include #include -#include -#include -#include -#include -#include namespace rmf_websocket { //============================================================================== diff --git a/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp b/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp index f41e79430..b27627fc8 100644 --- a/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp +++ b/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp @@ -15,216 +15,216 @@ * */ +#include +#include +#include +#include #include +#include #include #include +#include "utils/RingBuffer.hpp" +#include "client/ClientWebSocketEndpoint.hpp" +#include + namespace rmf_websocket { //============================================================================== class BroadcastClient::Implementation { public: - using WebsocketClient = - websocketpp::client; - using WebsocketMessagePtr = WebsocketClient::message_ptr; - using ConnectionHDL = websocketpp::connection_hdl; - using Connections = std::set>; - Implementation( const std::string& uri, const std::shared_ptr& node, ProvideJsonUpdates get_json_updates_cb) : _uri{std::move(uri)}, _node{std::move(node)}, - _queue_limit(1000), - _get_json_updates_cb{std::move(get_json_updates_cb)} + _get_json_updates_cb{std::move(get_json_updates_cb)}, + _queue(1000), + _io_service{}, + _endpoint(_uri, + _node, + &_io_service, + std::bind(&BroadcastClient::Implementation::on_connect, this)) { - _shutdown = false; - _connected = false; + _consumer_thread = std::thread([this]() + { + _io_service.run(); + }); - // Initialize the Asio transport policy - _client.clear_access_channels(websocketpp::log::alevel::all); - _client.clear_error_channels(websocketpp::log::elevel::all); - _client.init_asio(); - _client.start_perpetual(); - _client_thread = std::thread( - [c = this]() + _io_service.dispatch([this]() { - c->_client.run(); + _endpoint.connect(); }); + } - _client.set_open_handler( - [c = this](websocketpp::connection_hdl) - { - c->_connected = true; + //============================================================================ + Implementation(Implementation&& other) = delete; - if (c->_get_json_updates_cb) - c->publish(c->_get_json_updates_cb()); + //============================================================================ + Implementation& operator=(Implementation&& other) = delete; - RCLCPP_INFO( - c->_node->get_logger(), - "BroadcastClient successfully connected to uri: [%s]", - c->_uri.c_str()); - }); + //============================================================================ + Implementation(const Implementation& other) = delete; - _client.set_close_handler( - [c = this](websocketpp::connection_hdl) - { - c->_connected = false; - }); + //============================================================================ + Implementation operator=(const Implementation& other) = delete; - _client.set_fail_handler( - [c = this](websocketpp::connection_hdl) - { - c->_connected = false; - }); + //============================================================================ + void on_connect() + { + RCLCPP_INFO(_node->get_logger(), "Connected to server"); + + if (_get_json_updates_cb) + { + auto messages = _get_json_updates_cb(); - _processing_thread = std::thread( - [c = this]() + for (auto queue_item : messages) { - while (!c->_shutdown) + RCLCPP_INFO( + this->_node->get_logger(), "Sending initial message"); + auto status = _endpoint.get_status(); + if (!status.has_value()) { - // Try to connect to the server if we are not connected yet - if (!c->_connected) - { - websocketpp::lib::error_code ec; - WebsocketClient::connection_ptr con = c->_client.get_connection( - c->_uri, ec); - - if (con) - { - c->_hdl = con->get_handle(); - c->_client.connect(con); - // TOD(YV): Without sending a test payload, ec seems to be 0 even - // when the client has not connected. Avoid sending this message. - c->_client.send(c->_hdl, "Hello", - websocketpp::frame::opcode::text, - ec); - } - - if (!con || ec) - { - RCLCPP_WARN( - c->_node->get_logger(), - "BroadcastClient unable to connect to [%s]. Please make sure " - "server is running. Error msg: %s", - c->_uri.c_str(), - ec.message().c_str()); - c->_connected = false; - - { - std::lock_guard lock(c->_queue_mutex); - if (c->_queue_limit.has_value()) - { - if (c->_queue.size() > *c->_queue_limit) - { - RCLCPP_WARN( - c->_node->get_logger(), - "Reducing size of broadcast queue from [%lu] down to " - "its limit of [%lu]", - c->_queue.size(), - *c->_queue_limit); - - while (c->_queue.size() > *c->_queue_limit) - c->_queue.pop(); - } - } - } - - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - continue; - } - - RCLCPP_INFO( - c->_node->get_logger(), - "BroadcastClient successfully connected to [%s]", - c->_uri.c_str()); - c->_connected = true; - } + log("Endpoint has not yet been initiallized."); + return; + } - std::unique_lock lock(c->_wait_mutex); - c->_cv.wait(lock, - [c]() - { - return !c->_queue.empty(); - }); + if (status != ConnectionMetadata::ConnectionStatus::OPEN) + { + // Attempt reconnect + log("Disconnected during init."); + return; + } - while (!c->_queue.empty()) - { - std::lock_guard lock(c->_queue_mutex); - websocketpp::lib::error_code ec; - const std::string& msg = c->_queue.front().dump(); - c->_client.send(c->_hdl, msg, websocketpp::frame::opcode::text, ec); - if (ec) - { - RCLCPP_ERROR( - c->_node->get_logger(), - "BroadcastClient unable to publish message: %s", - ec.message().c_str()); - // TODO(YV): Check if we should re-connect to server - break; - } - c->_queue.pop(); - } + // Send + auto ec = _endpoint.send(queue_item.dump()); + if (ec) + { + log("Send failed. Attempting reconnection."); + return; } + } + RCLCPP_INFO( + this->_node->get_logger(), + "Sent all updates"); + } + RCLCPP_INFO( + this->_node->get_logger(), + "Attempting queue flush if connected"); + _io_service.dispatch([this]() + { + _flush_queue_if_connected(); }); } - // Publish a single message + //============================================================================ + void log(const std::string& str) + { + RCLCPP_ERROR( + this->_node->get_logger(), + "%s", + str.c_str() + ); + } + + //============================================================================ void publish(const nlohmann::json& msg) { - std::lock_guard lock(_queue_mutex); + /// _queue is thread safe. No need to lock. _queue.push(msg); - _cv.notify_all(); + _io_service.dispatch([this]() + { + _flush_queue_if_connected(); + }); } - // Publish a vector of messages + //============================================================================ void publish(const std::vector& msgs) { - std::lock_guard lock(_queue_mutex); - for (const auto& msg : msgs) - _queue.push(msg); - _cv.notify_all(); + for (auto msg: msgs) + { + bool full = _queue.push(msg); + if (full) + { + log("Buffer full dropping oldest message"); + } + } + _io_service.dispatch([this]() + { + _flush_queue_if_connected(); + }); } + //============================================================================ void set_queue_limit(std::optional limit) { - std::lock_guard lock(_queue_mutex); - _queue_limit = limit; + /// _queue is thread safe. No need to lock. + if (limit.has_value()) + _queue.resize(limit.value()); } + //============================================================================ ~Implementation() { - _shutdown = true; - if (_processing_thread.joinable()) - { - _processing_thread.join(); - } - if (_client_thread.joinable()) - { - _client_thread.join(); - } - _client.stop_perpetual(); + _io_service.stop(); + _consumer_thread.join(); } private: + //============================================================================ + void _flush_queue_if_connected() + { + while (!_queue.empty()) + { + auto status = _endpoint.get_status(); + if (!status.has_value()) + { + log("Endpoint has not yet been initiallized."); + return; + } + if (status != ConnectionMetadata::ConnectionStatus::OPEN) + { + log("Connection not yet established"); + return; + } + auto queue_item = _queue.front(); + if (!queue_item.has_value()) + { + // Technically this should be unreachable as long as the client is + // single threaded + throw std::runtime_error( + "The queue was modified when it shouldnt have been"); + return; + } + auto ec = _endpoint.send(queue_item->dump()); + if (ec) + { + log("Sending message failed. Maybe due to intermediate disconnection"); + return; + } + else + { + RCLCPP_INFO( + this->_node->get_logger(), "Sent successfully"); + } + _queue.pop_item(); + } + RCLCPP_INFO( + this->_node->get_logger(), "Emptied queue"); + } // create pimpl std::string _uri; + boost::asio::io_service _io_service; std::shared_ptr _node; - std::optional _queue_limit; - WebsocketClient _client; - websocketpp::connection_hdl _hdl; - std::mutex _wait_mutex; - std::mutex _queue_mutex; - std::condition_variable _cv; - std::queue _queue; - std::thread _processing_thread; - std::thread _client_thread; - std::atomic_bool _connected; - std::atomic_bool _shutdown; + RingBuffer _queue; ProvideJsonUpdates _get_json_updates_cb; + std::atomic _stop; + ClientWebSocketEndpoint _endpoint; + + std::thread _consumer_thread; }; //============================================================================== diff --git a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp index 99a823ba4..78eb0fd48 100644 --- a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp +++ b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp @@ -139,25 +139,30 @@ websocketpp::lib::error_code ClientWebSocketEndpoint::connect() auto reconnect_socket = [this]() { // TODO(arjo) Parametrize the timeout. - RCLCPP_ERROR(_node->get_logger(), - "Connection lost\n" - "> Reconnecting in 1s\n" - "> Host: %s", _uri.c_str()); - _endpoint->stop_perpetual(); - auto io_service = &_endpoint->get_io_service(); - _endpoint = std::make_unique(); - _endpoint->clear_access_channels(websocketpp::log::alevel::all); - _endpoint->clear_error_channels(websocketpp::log::elevel::all); - _endpoint->init_asio(io_service); - _endpoint->start_perpetual(); - websocketpp::lib::error_code ec; - - _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, - this)); - + using namespace std::chrono_literals; + if (!_reconnect_enqueued) + { + RCLCPP_ERROR(_node->get_logger(), + "Connection lost\n" + "> Reconnecting in 1s\n" + "> Host: %s", _uri.c_str()); + _endpoint->stop_perpetual(); + auto io_service = &_endpoint->get_io_service(); + _endpoint = std::make_unique(); + _endpoint->clear_access_channels(websocketpp::log::alevel::all); + _endpoint->clear_error_channels(websocketpp::log::elevel::all); + _endpoint->init_asio(io_service); + _endpoint->start_perpetual(); + websocketpp::lib::error_code ec; + + _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, + this)); + _reconnect_enqueued = true; + } }; auto connected_cb = [this]() { + _reconnect_enqueued = false; _connection_cb(); }; diff --git a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.hpp b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.hpp new file mode 100644 index 000000000..55f7105c3 --- /dev/null +++ b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.hpp @@ -0,0 +1,150 @@ +#ifndef RMF_WEBSOCKET__CLIENT_CLIENTWEBSOCKETENDPOINT_HPP +#define RMF_WEBSOCKET__CLIENT_CLIENTWEBSOCKETENDPOINT_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace rmf_websocket { + +/// Client +typedef websocketpp::client WsClient; + + +/// Helper class with event handlers for managing connection state. +class ConnectionMetadata +{ +public: + /// Connection callback + typedef std::function ConnectionCallback; + + /// Connection callback + typedef std::function ReconnectionCallback; + + /// Connection Status + enum class ConnectionStatus + { + CONNECTING, + OPEN, + FAILED, + CLOSED + }; + + typedef websocketpp::lib::shared_ptr ptr; + + /// Constuctor + ConnectionMetadata( + websocketpp::connection_hdl hdl, + std::string uri, + ConnectionCallback cb, + ReconnectionCallback rcb); + + /// On open event handler + void on_open(WsClient* c, websocketpp::connection_hdl hdl); + + /// On fail event handler + void on_fail(WsClient* c, websocketpp::connection_hdl hdl); + + /// On close event handler + void on_close(WsClient* c, websocketpp::connection_hdl hdl); + + /// Get status + ConnectionStatus get_status() const; + + /// Get debug string + std::string debug_data() const; + + /// Get connection handle + websocketpp::connection_hdl get_hdl() const; + + /// reset + void reset(); + + friend std::ostream& operator<<(std::ostream& out, + ConnectionMetadata const& data); +private: + websocketpp::connection_hdl _hdl; + WsClient::connection_ptr _con; + ConnectionStatus _status; + std::string _uri; + std::string _server; + std::string _error_reason; + ConnectionCallback _connection_cb; + ReconnectionCallback _reconnection_cb; +}; + + +/// This classs abstracts out reconnecting to an end point. +class ClientWebSocketEndpoint +{ +public: + typedef std::function ConnectionCallback; + /// Constructor + /// Pass io service so that multiple endpoints + /// can run on the same thread + ClientWebSocketEndpoint( + std::string const& uri, + std::shared_ptr node, + boost::asio::io_service* io_service, + ConnectionCallback cb); + + /// Delete move constructor + ClientWebSocketEndpoint( + ClientWebSocketEndpoint&& other) = delete; + + /// Delete move assignment + ClientWebSocketEndpoint& operator=( + ClientWebSocketEndpoint&& other) = delete; + + /// Delete copy constructor + ClientWebSocketEndpoint( + const ClientWebSocketEndpoint& other) = delete; + + /// Delete copy + ClientWebSocketEndpoint operator=( + const ClientWebSocketEndpoint& other) = delete; + + /// Initiates a connection returns 0 if everything goes ok. + /// Note: This is non blocking and does not gaurantee a connection + /// has been established. + websocketpp::lib::error_code connect(); + + /// Gets the current connection metadata. This includes the current + /// link state. + std::optional get_status() const; + + /// Send a message. + websocketpp::lib::error_code send(const std::string& message); + + /// Destructor + ~ClientWebSocketEndpoint(); + +private: + std::unique_ptr _endpoint; + std::atomic _stop; + + ConnectionMetadata::ptr _current_connection; + std::string _uri; + std::shared_ptr _node; + WsClient::connection_ptr _con; + bool _init, _enqueued_conn, _reconnect_enqueued; + ConnectionCallback _connection_cb; +}; +} +#endif diff --git a/rmf_websocket/src/rmf_websocket/utils/RingBuffer.hpp b/rmf_websocket/src/rmf_websocket/utils/RingBuffer.hpp new file mode 100644 index 000000000..1a7e65659 --- /dev/null +++ b/rmf_websocket/src/rmf_websocket/utils/RingBuffer.hpp @@ -0,0 +1,81 @@ +#ifndef RMF_WEBSOCKET__UTILS_RINGBUFFER_HPP +#define RMF_WEBSOCKET__UTILS_RINGBUFFER_HPP + +#include +#include +#include +#include +#include + +namespace rmf_websocket { + +//============================================================================== +/// Thread safe fixed sized ring buffer +template +class RingBuffer +{ + +//============================================================================== +public: RingBuffer(std::size_t size) + : _vec(size) + { + + } + +//============================================================================== +/// Resize the capacity of the wing buffer +public: void resize(std::size_t buffer) + { + std::lock_guard lock(_mtx); + _vec.set_capacity(buffer); + } + +//============================================================================== +/// Push an item onto the queue +public: bool push(T item) + { + std::lock_guard lock(_mtx); + auto full = _vec.full(); + _vec.push_back(std::move(item)); + return !full; + } + +//============================================================================== +public: bool empty() + { + std::lock_guard lock(_mtx); + return _vec.empty(); + } +//============================================================================== +public: std::optional front() + { + std::lock_guard lock(_mtx); + if (_vec.empty()) + { + return std::nullopt; + } + return _vec.front(); + } +//============================================================================== +public: std::optional pop_item() + { + std::lock_guard lock(_mtx); + if (_vec.empty()) + { + return std::nullopt; + } + + T item = _vec.front(); + _vec.pop_front(); + + return item; + } + +private: + boost::circular_buffer _vec; + std::mutex _mtx; +}; + +} + +#endif diff --git a/rmf_websocket/src/rmf_websocket/utils/RingBuffer_TEST.cpp b/rmf_websocket/src/rmf_websocket/utils/RingBuffer_TEST.cpp new file mode 100644 index 000000000..8d2c6f290 --- /dev/null +++ b/rmf_websocket/src/rmf_websocket/utils/RingBuffer_TEST.cpp @@ -0,0 +1,36 @@ +#define CATCH_CONFIG_MAIN +#include + +#include +#include "RingBuffer.hpp" + +using namespace rmf_websocket; + +TEST_CASE("RingBuffer pops item in correct order", "[RingBuffer]") { + RingBuffer buffer(10); + + REQUIRE(buffer.pop_item() == std::nullopt); + + buffer.push(1); + buffer.push(2); + buffer.push(3); + + REQUIRE(buffer.pop_item().value() == 1); + REQUIRE(buffer.pop_item().value() == 2); + REQUIRE(buffer.pop_item().value() == 3); + REQUIRE(buffer.pop_item() == std::nullopt); +} + + +TEST_CASE("RingBuffer overwrites old values in correct order", "[RingBuffer]") { + RingBuffer buffer(2); + + REQUIRE(buffer.pop_item() == std::nullopt); + + buffer.push(1); + buffer.push(2); + buffer.push(3); + REQUIRE(buffer.pop_item().value() == 2); + REQUIRE(buffer.pop_item().value() == 3); + REQUIRE(buffer.pop_item() == std::nullopt); +} diff --git a/rmf_websocket/test/test_client.cpp b/rmf_websocket/test/test_client.cpp new file mode 100644 index 000000000..6f340f401 --- /dev/null +++ b/rmf_websocket/test/test_client.cpp @@ -0,0 +1,115 @@ +#include +#define CATCH_CONFIG_MAIN +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +typedef websocketpp::server server; +typedef server::message_ptr message_ptr; + +using websocketpp::lib::placeholders::_1; +using websocketpp::lib::placeholders::_2; +using websocketpp::lib::bind; + + +std::atomic_bool terminate_server = false; +std::atomic_bool timed_out = false; +std::atomic num_msgs = 0; +std::vector msgs; +// Define a handler for incoming messages +void on_message(server* s, websocketpp::connection_hdl hdl, message_ptr msg) +{ + terminate_server = true; + num_msgs++; + msgs.push_back(nlohmann::json::parse(msg->get_payload())); + server::connection_ptr con = s->get_con_from_hdl(hdl); + con->close(websocketpp::close::status::normal, ""); +} + + +void run_server() +{ + server echo_server; + + echo_server.set_access_channels(websocketpp::log::alevel::all); + echo_server.clear_access_channels(websocketpp::log::alevel::frame_payload); + echo_server.init_asio(); + echo_server.set_reuse_addr(true); + // Set on_message handler + echo_server.set_message_handler( + bind(&on_message, &echo_server, ::_1, ::_2)); + + + // Set the port number + echo_server.listen(9000); + + // Start the server asynchronously + echo_server.start_accept(); + + // Run the server loop + while (!terminate_server) + { + echo_server.run_one(); + } + + echo_server.stop_listening(); + echo_server.run(); +} + + +TEST_CASE("Client", "Reconnecting server") { + rclcpp::init(0, {}); + auto test_node = std::make_shared("test_node"); + + auto t1 = std::thread([]() + { + run_server(); + }); + auto broadcaster = rmf_websocket::BroadcastClient::make( + "ws://localhost:9000/", test_node); + + nlohmann::json jsonString; + jsonString["test"] = "1"; + broadcaster->publish(jsonString); + t1.join(); + + std::cout << "Restart server\n"; + + REQUIRE(num_msgs == 1); + + terminate_server = false; + auto t2 = std::thread([]() + { + run_server(); + }); + + + jsonString["test"] = "2"; + auto tries = 0; + + using namespace std::chrono_literals; + while (tries < 10 && !terminate_server) + { + /// TODO(arjoc): Make timeout reconfigureable, that + /// way we dont need to wait so long. + broadcaster->publish(jsonString); + std::this_thread::sleep_for(1000ms); + ++tries; + } + t2.join(); + + REQUIRE(num_msgs == 2); + + REQUIRE(msgs[0]["test"] == "1"); + REQUIRE(msgs[1]["test"] == "2"); + +} diff --git a/rmf_websocket/test/test_client_no_server.cpp b/rmf_websocket/test/test_client_no_server.cpp new file mode 100644 index 000000000..829aded1e --- /dev/null +++ b/rmf_websocket/test/test_client_no_server.cpp @@ -0,0 +1,21 @@ +#include +#define CATCH_CONFIG_MAIN +#include + +#include +#include +#include +#include +#include +#include +#include + +TEST_CASE("Client", "Reconnecting server") { + rclcpp::init(0, {}); + auto test_node = std::make_shared("test_node"); + auto broadcaster = rmf_websocket::BroadcastClient::make( + "ws://localhost:9000/", test_node); + nlohmann::json jsonString; + jsonString["test"] = "1"; + broadcaster->publish(jsonString); +} diff --git a/rmf_websocket/test/test_client_with_update_cb.cpp b/rmf_websocket/test/test_client_with_update_cb.cpp new file mode 100644 index 000000000..94bce4e34 --- /dev/null +++ b/rmf_websocket/test/test_client_with_update_cb.cpp @@ -0,0 +1,141 @@ +#include +#include +#define CATCH_CONFIG_MAIN +#include + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +typedef websocketpp::server server; +typedef server::message_ptr message_ptr; + +using websocketpp::lib::placeholders::_1; +using websocketpp::lib::placeholders::_2; +using websocketpp::lib::bind; + + +std::atomic_bool terminate_server = false; +std::atomic_bool timed_out = false; +std::atomic num_msgs = 0; +std::atomic num_init_msgs = 0; +std::promise on_init_promise; + +using namespace std::chrono_literals; + +std::vector msgs; +// Define a handler for incoming messages +void on_message(std::shared_ptr s, websocketpp::connection_hdl hdl, + message_ptr msg) +{ + auto json = nlohmann::json::parse(msg->get_payload()); + if (json["test"] == "init") + { + num_init_msgs++; + return; + } + num_msgs++; + msgs.push_back(json); + terminate_server = true; + std::cout << "message\n"; + server::connection_ptr con = s->get_con_from_hdl(hdl); + con->close(websocketpp::close::status::normal, ""); +} + + +void run_server() +{ + std::shared_ptr echo_server = + std::make_shared(); + + std::cout << "Reuse server\n"; + echo_server->set_access_channels(websocketpp::log::alevel::all); + echo_server->clear_access_channels(websocketpp::log::alevel::frame_payload); + echo_server->init_asio(); + echo_server->set_reuse_addr(true); + // Set on_message handler + echo_server->set_message_handler( + bind(&on_message, echo_server, ::_1, ::_2)); + + + // Set the port number + echo_server->listen(9000); + + // Start the server asynchronously + echo_server->start_accept(); + + + // Run the server loop + while (!terminate_server) + { + echo_server->run_one(); + } + + echo_server->stop_listening(); + echo_server->run(); +} + + +std::vector init_function() +{ + nlohmann::json json; + json["test"] = "init"; + std::vector msgs; + msgs.push_back(json); + on_init_promise.set_value(); + std::cout << "init\n"; + return msgs; +} + +TEST_CASE("Client", "Reconnecting server") { + + rclcpp::init(0, {}); + auto test_node = std::make_shared("test_node"); + + auto t1 = std::thread([]() + { + run_server(); + }); + auto broadcaster = rmf_websocket::BroadcastClient::make( + "ws://localhost:9000/", test_node, init_function); + + nlohmann::json jsonString; + jsonString["test"] = "1"; + broadcaster->publish(jsonString); + t1.join(); + + + terminate_server = false; + + + REQUIRE(num_msgs == 1); + REQUIRE(num_init_msgs == 1); + + on_init_promise = std::promise(); + + auto t2 = std::thread([]() + { + run_server(); + }); + jsonString["test"] = "2"; + broadcaster->publish(jsonString); + + t2.join(); + + + REQUIRE_NOTHROW(on_init_promise.get_future().wait_for(5s)); + + REQUIRE(num_msgs == 2); + + REQUIRE(msgs[0]["test"] == "1"); + REQUIRE(msgs[1]["test"] == "2"); + REQUIRE(num_init_msgs >= 2); +} From 0ded63bdfa56118742ae31d0924460b93ca3e0df Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Apr 2024 15:55:30 +0800 Subject: [PATCH 047/119] Lower debug level of some messages in rmf_websocket (#340) Signed-off-by: Arjo Chakravarty --- rmf_websocket/src/rmf_websocket/BroadcastClient.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp b/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp index b27627fc8..01d3ac1be 100644 --- a/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp +++ b/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp @@ -207,12 +207,12 @@ class BroadcastClient::Implementation } else { - RCLCPP_INFO( + RCLCPP_DEBUG( this->_node->get_logger(), "Sent successfully"); } _queue.pop_item(); } - RCLCPP_INFO( + RCLCPP_DEBUG( this->_node->get_logger(), "Emptied queue"); } // create pimpl From 61f712a8d1dd19be9813aeeefbde43fd982c6be5 Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 17 Apr 2024 18:28:53 +0800 Subject: [PATCH 048/119] Manual release of mutex groups (#339) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/StandardNames.hpp | 2 ++ .../agv/FleetUpdateHandle.cpp | 17 ++++++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 33 +++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 27 ++++++++++++++- 4 files changed, 78 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 310812928..840ab8a18 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -73,6 +73,8 @@ const std::string ChargingAssignmentsTopicName = "charging_assignments"; const std::string MutexGroupRequestTopicName = "mutex_group_request"; const std::string MutexGroupStatesTopicName = "mutex_group_states"; +const std::string MutexGroupManualReleaseTopicName = + "mutex_group_manual_release"; const std::string ReservationRequestTopicName = "/rmf/reservations/request"; const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 2afd86eee..a080d8f99 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1285,6 +1285,23 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const commission.is_performing_idle_behavior(); json["commission"] = commission_json; + + nlohmann::json mutex_groups_json; + std::vector locked_mutex_groups; + for (const auto& g : context->locked_mutex_groups()) + { + locked_mutex_groups.push_back(g.first); + } + mutex_groups_json["locked"] = std::move(locked_mutex_groups); + + std::vector requesting_mutex_groups; + for (const auto& g : context->requesting_mutex_groups()) + { + requesting_mutex_groups.push_back(g.first); + } + mutex_groups_json["requesting"] = std::move(requesting_mutex_groups); + + json["mutex_groups"] = std::move(mutex_groups_json); } try diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 5fc732efb..2faa8340f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1079,6 +1079,13 @@ RobotContext::locked_mutex_groups() const return _locked_mutex_groups; } +//============================================================================== +const std::unordered_map& +RobotContext::requesting_mutex_groups() const +{ + return _requesting_mutex_groups; +} + //============================================================================== const rxcpp::observable& RobotContext::request_mutex_groups( std::unordered_set groups, @@ -1506,6 +1513,7 @@ void RobotContext::_check_mutex_groups( } } +//============================================================================== void RobotContext::_retain_mutex_groups( const std::unordered_set& retain, std::unordered_map& groups) @@ -1639,5 +1647,30 @@ std::unordered_set RobotContext::_get_free_spots() const } return set; } + +//============================================================================== +void RobotContext::_handle_mutex_group_manual_release( + const rmf_fleet_msgs::msg::MutexGroupManualRelease& msg) +{ + if (msg.fleet != group()) + return; + + if (msg.robot != name()) + return; + + std::unordered_set retain; + for (const auto& g : _locked_mutex_groups) + { + retain.insert(g.first); + } + + for (const auto& g : msg.release_mutex_groups) + { + retain.erase(g); + } + + retain_mutex_groups(retain); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index b20559015..2f5deb307 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -23,6 +23,9 @@ #include #include #include +#include + +#include #include #include @@ -677,9 +680,14 @@ class RobotContext /// Check if a door is being held const std::optional& holding_door() const; - /// What mutex group is currently being locked. + /// What mutex groups are currently locked by this robot. const std::unordered_map& locked_mutex_groups() const; + /// What mutex groups are currently being requested (but have not yet been + /// locked) by this robot. + const std::unordered_map& + requesting_mutex_groups() const; + /// Set the mutex group that this robot needs to lock. const rxcpp::observable& request_mutex_groups( std::unordered_set groups, @@ -793,6 +801,19 @@ class RobotContext self->_free_spots = *msg; }); + context->_mutex_group_manual_release_sub = + context->_node->create_subscription< + rmf_fleet_msgs::msg::MutexGroupManualRelease>( + MutexGroupManualReleaseTopicName, + rclcpp::SystemDefaultsQoS() + .reliable() + .keep_last(10), + [w = context->weak_from_this()]( + rmf_fleet_msgs::msg::MutexGroupManualRelease::SharedPtr msg) + { + if (const auto self = w.lock()) + self->_handle_mutex_group_manual_release(*msg); + }); return context; } @@ -899,6 +920,8 @@ class RobotContext std::unordered_map& _groups); void _release_mutex_group(const MutexGroupData& data) const; void _publish_mutex_group_requests(); + void _handle_mutex_group_manual_release( + const rmf_fleet_msgs::msg::MutexGroupManualRelease& msg); std::unordered_map _requesting_mutex_groups; std::unordered_map _locked_mutex_groups; @@ -909,6 +932,8 @@ class RobotContext rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; rmf_rxcpp::subscription_guard _free_space_sub; + rclcpp::Subscription::SharedPtr + _mutex_group_manual_release_sub; std::chrono::steady_clock::time_point _last_active_task_time; uint64_t _last_reservation_request_id; ReservationManager _reservation_mgr; From 24ca1acee184307d59773afdabd90f0af0aa3db6 Mon Sep 17 00:00:00 2001 From: Xiyu Date: Thu, 18 Apr 2024 10:12:25 +0800 Subject: [PATCH 049/119] Disable automatic retreat (#330) * Add retreat_to_charger param Signed-off-by: Xiyu Oh * If idle task is charge, do not retreat to charger Signed-off-by: Xiyu Oh * Use duration instead of boolean for retreat to charger Signed-off-by: Xiyu Oh * Undo adding request type Signed-off-by: Xiyu Oh * Change retreat to timer from bool to optional duration Signed-off-by: Xiyu Oh * Store param in FleetUpdateHandle Signed-off-by: Xiyu Oh * Uncrustify and remove request_type from ChargeBattery Signed-off-by: Xiyu Oh * Tweak some details and remove API breakages Signed-off-by: Michael X. Grey * Fix style Signed-off-by: Michael X. Grey --------- Signed-off-by: Xiyu Oh Signed-off-by: Michael X. Grey Co-authored-by: Michael X. Grey Co-authored-by: Grey Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 14 +++++ .../agv/FleetUpdateHandle.hpp | 10 ++++ .../src/rmf_fleet_adapter/TaskManager.cpp | 45 +++++++++++---- .../src/rmf_fleet_adapter/TaskManager.hpp | 6 ++ .../src/rmf_fleet_adapter/agv/Adapter.cpp | 3 + .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 56 +++++++++++++++++++ .../agv/FleetUpdateHandle.cpp | 22 ++++++++ .../agv/internal_FleetUpdateHandle.hpp | 2 + rmf_fleet_adapter_python/src/adapter.cpp | 7 +++ 9 files changed, 155 insertions(+), 10 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index 2be6c0698..c6eb2cd30 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -513,6 +513,12 @@ class EasyFullControl::FleetConfiguration /// vehicles in this fleet when battery levels fall below the /// recharge_threshold. /// + /// \param[in] retreat_to_charger_interval + /// Specify whether to allow automatic retreat to charger if the robot's + /// battery is estimated to fall below its recharge_threshold before it is + /// able to complete its current task. Provide a duration between checks in + /// seconds. If nullopt, retreat to charger would be disabled. + /// /// \param[in] task_categories /// Provide callbacks for considering tasks belonging to each category. /// @@ -706,6 +712,14 @@ class EasyFullControl::FleetConfiguration /// Set whether or not to account for battery drain during task planning. void set_account_for_battery_drain(bool value); + /// Get the duration between retreat to charger checks. + std::optional retreat_to_charger_interval() const; + + /// Set the duration between retreat to charger checks. Passing in a nullopt + /// will turn off these checks entirely. + void set_retreat_to_charger_interval( + std::optional value); + /// Get the task categories const std::unordered_map& task_consideration() const; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 290f5bd35..4c536bd98 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -391,6 +391,16 @@ class FleetUpdateHandle : public std::enable_shared_from_this FleetUpdateHandle& set_update_listener( std::function listener); + /// Get the duration between retreat to charger checks. + std::optional retreat_to_charger_interval() const; + + /// Specify whether to allow automatic retreat to charger if the robot's + /// battery is estimated to fall below its recharge_threshold before it is + /// able to complete its current task. Provide a duration between checks in + /// seconds. If nullopt, retreat to charger would be disabled. + FleetUpdateHandle& set_retreat_to_charger_interval( + std::optional duration); + /// Get the rclcpp::Node that this fleet update handle will be using for /// communication. std::shared_ptr node(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 78a50f5c3..1f4c71b24 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -162,16 +162,6 @@ TaskManagerPtr TaskManager::make( } }); - mgr->_retreat_timer = mgr->context()->node()->try_create_wall_timer( - std::chrono::seconds(10), - [w = mgr->weak_from_this()]() - { - if (auto mgr = w.lock()) - { - mgr->retreat_to_charger(); - } - }); - mgr->_begin_waiting(); // TODO(MXG): The tests allow a task manager to be created before a task @@ -1752,6 +1742,41 @@ std::function TaskManager::_make_resume_from_waiting() }; } +//============================================================================== +void TaskManager::configure_retreat_to_charger( + std::optional duration) +{ + if (duration.has_value() && *duration <= rmf_traffic::Duration(0)) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "[TaskManager::configure_retreat_to_charger] " + "Invalid value for duration: %f", + rmf_traffic::time::to_seconds(*duration)); + } + + if (!duration.has_value() || *duration <= rmf_traffic::Duration(0)) + { + if (_retreat_timer && !_retreat_timer->is_canceled()) + { + _retreat_timer->cancel(); + } + return; + } + + if (_retreat_timer) + _retreat_timer->reset(); + _retreat_timer = _context->node()->try_create_wall_timer( + duration.value(), + [w = weak_from_this()]() + { + if (auto mgr = w.lock()) + { + mgr->retreat_to_charger(); + } + }); +} + //============================================================================== void TaskManager::retreat_to_charger() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 7a409a7e7..e2b2c87f4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -156,6 +156,12 @@ class TaskManager : public std::enable_shared_from_this /// when robot is idle and battery level drops below a retreat threshold. void retreat_to_charger(); + /// Start the retreat timer that periodically checks whether the robot + /// should retreat to charger if its battery state of charge is close to + /// the recharge threshold. + void configure_retreat_to_charger( + std::optional duration); + /// Get the list of task ids for tasks that have started execution. /// The list will contain upto 100 latest task ids only. const std::vector& get_executed_tasks() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index 2bb577397..9c1e4d5dc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -317,6 +317,9 @@ std::shared_ptr Adapter::add_easy_fleet( config.fleet_name().c_str()); } + fleet_handle->set_retreat_to_charger_interval( + config.retreat_to_charger_interval()); + for (const auto& [task, consider] : config.task_consideration()) { if (task == "delivery" && consider) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index 780deab8f..b44238d45 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1689,6 +1689,7 @@ class EasyFullControl::FleetConfiguration::Implementation double recharge_threshold; double recharge_soc; bool account_for_battery_drain; + std::optional retreat_to_charger_interval; std::unordered_map task_consideration; std::unordered_map action_consideration; rmf_task::ConstRequestFactoryPtr finishing_request; @@ -1744,6 +1745,7 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(recharge_threshold), std::move(recharge_soc), std::move(account_for_battery_drain), + std::chrono::seconds(10), std::move(task_consideration), std::move(action_consideration), std::move(finishing_request), @@ -1971,6 +1973,45 @@ EasyFullControl::FleetConfiguration::from_config_files( { recharge_soc = rmf_fleet["recharge_soc"].as(); } + // Retreat to charger + std::optional retreat_to_charger_interval = + rmf_traffic::time::from_seconds(10); + const auto retreat_to_charger_yaml = rmf_fleet["retreat_to_charger_interval"]; + if (!retreat_to_charger_yaml) + { + std::cout << "[retreat_to_charger_interval] value is not provided, " + << "default to 10 seconds" << std::endl; + } + else if (retreat_to_charger_yaml.IsScalar()) + { + const double retreat_to_charger_interval_sec = + retreat_to_charger_yaml.as(); + if (retreat_to_charger_interval_sec <= 0.0) + { + std::cout + << "[retreat_to_charger_interval] has invalid value [" + << retreat_to_charger_interval_sec << "]. Turning off retreat to " + << "behavior." << std::endl; + retreat_to_charger_interval = std::nullopt; + } + else + { + retreat_to_charger_interval = + rmf_traffic::time::from_seconds(retreat_to_charger_interval_sec); + } + } + else if (retreat_to_charger_yaml.IsNull()) + { + retreat_to_charger_interval = std::nullopt; + } + else + { + const auto mark = retreat_to_charger_yaml.Mark(); + std::cout << "[retreat_to_charger_interval] Unsupported value type " + << "provided: line " << mark.line << ", column " << mark.column + << std::endl; + return std::nullopt; + } // Task capabilities std::unordered_map task_consideration; @@ -2322,6 +2363,7 @@ EasyFullControl::FleetConfiguration::from_config_files( default_max_merge_lane_distance, default_min_lane_length); config.change_lift_emergency_levels() = lift_emergency_levels; + config.set_retreat_to_charger_interval(retreat_to_charger_interval); return config; } @@ -2519,6 +2561,20 @@ void EasyFullControl::FleetConfiguration::set_account_for_battery_drain( _pimpl->account_for_battery_drain = value; } +//============================================================================== +std::optional +EasyFullControl::FleetConfiguration::retreat_to_charger_interval() const +{ + return _pimpl->retreat_to_charger_interval; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_retreat_to_charger_interval( + std::optional value) +{ + _pimpl->retreat_to_charger_interval = value; +} + //============================================================================== const std::unordered_map& EasyFullControl::FleetConfiguration::task_consideration() const diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index a080d8f99..f121e300c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1911,6 +1911,7 @@ void FleetUpdateHandle::add_robot( } mgr->set_idle_task(fleet->_pimpl->idle_task); + mgr->configure_retreat_to_charger(fleet->retreat_to_charger_interval()); // -- Calling the handle_cb should always happen last -- if (handle_cb) @@ -2516,6 +2517,27 @@ FleetUpdateHandle& FleetUpdateHandle::set_update_listener( return *this; } +//============================================================================== +std::optional +FleetUpdateHandle::retreat_to_charger_interval() const +{ + return _pimpl->retreat_to_charger_interval; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::set_retreat_to_charger_interval( + std::optional duration) +{ + _pimpl->retreat_to_charger_interval = duration; + + // Start retreat timer + for (const auto& t : _pimpl->task_managers) + { + t.second->configure_retreat_to_charger(duration); + } + return *this; +} + //============================================================================== std::shared_ptr FleetUpdateHandle::node() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 2e218e882..7b5800445 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -288,6 +288,8 @@ class FleetUpdateHandle::Implementation rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); + rmf_utils::optional retreat_to_charger_interval = + std::chrono::nanoseconds(std::chrono::seconds(10)); AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map Date: Tue, 23 Apr 2024 10:06:29 +0800 Subject: [PATCH 050/119] Fix deadlock in websocket server (#342) This is a rather dumb mistake. I think I left this in as a vestige of my testing. However, the websocket client was refusing to reconnect thanks to a deadlock caused by this variable. Signed-off-by: Arjo Chakravarty --- .../client/ClientWebSocketEndpoint.cpp | 37 ++++++++----------- 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp index 78eb0fd48..99a823ba4 100644 --- a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp +++ b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp @@ -139,30 +139,25 @@ websocketpp::lib::error_code ClientWebSocketEndpoint::connect() auto reconnect_socket = [this]() { // TODO(arjo) Parametrize the timeout. - using namespace std::chrono_literals; - if (!_reconnect_enqueued) - { - RCLCPP_ERROR(_node->get_logger(), - "Connection lost\n" - "> Reconnecting in 1s\n" - "> Host: %s", _uri.c_str()); - _endpoint->stop_perpetual(); - auto io_service = &_endpoint->get_io_service(); - _endpoint = std::make_unique(); - _endpoint->clear_access_channels(websocketpp::log::alevel::all); - _endpoint->clear_error_channels(websocketpp::log::elevel::all); - _endpoint->init_asio(io_service); - _endpoint->start_perpetual(); - websocketpp::lib::error_code ec; - - _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, - this)); - _reconnect_enqueued = true; - } + RCLCPP_ERROR(_node->get_logger(), + "Connection lost\n" + "> Reconnecting in 1s\n" + "> Host: %s", _uri.c_str()); + _endpoint->stop_perpetual(); + auto io_service = &_endpoint->get_io_service(); + _endpoint = std::make_unique(); + _endpoint->clear_access_channels(websocketpp::log::alevel::all); + _endpoint->clear_error_channels(websocketpp::log::elevel::all); + _endpoint->init_asio(io_service); + _endpoint->start_perpetual(); + websocketpp::lib::error_code ec; + + _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, + this)); + }; auto connected_cb = [this]() { - _reconnect_enqueued = false; _connection_cb(); }; From caf32bf1a17117efc85c2ca6219e0f01c19d0a46 Mon Sep 17 00:00:00 2001 From: Grey Date: Thu, 25 Apr 2024 15:13:35 +0800 Subject: [PATCH 051/119] Automatically begin or cancel idle behavior when commission changes (#346) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/TaskManager.cpp | 23 +++++++++++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 13 ++++++++--- .../rmf_fleet_adapter/agv/RobotContext.cpp | 22 ++++++++++++++++-- 3 files changed, 51 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 1f4c71b24..947230885 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -1356,6 +1356,15 @@ bool TaskManager::kill_task( return false; } +//============================================================================== +void TaskManager::_cancel_idle_behavior(std::vector labels) +{ + if (_waiting) + { + _waiting.cancel(std::move(labels), _context->now()); + } +} + //============================================================================== void TaskManager::_begin_next_task() { @@ -1373,7 +1382,6 @@ void TaskManager::_begin_next_task() if (_queue.empty() && _direct_queue.empty()) { - if (!_waiting && !_finished_waiting) { _begin_waiting(); @@ -1470,7 +1478,9 @@ void TaskManager::_begin_next_task() else { if (!_waiting && !_finished_waiting) + { _begin_waiting(); + } } _context->worker().schedule( @@ -1730,7 +1740,16 @@ std::function TaskManager::_make_resume_from_waiting() if (!self) return; - self->_finished_waiting = true; + // This condition deals with an awkward edge case where idle behavior + // would not restart when toggling the idle behavior commission back + // on. We fix this by keeping the _finished_waiting flag clean if + // idle behavior commissioning is off, so there's nothing to block + // idle behavior from beginning again if it gets toggled back on. + if (self->_context->commission().is_performing_idle_behavior()) + { + self->_finished_waiting = true; + } + self->_waiting = ActiveTask(); self->_begin_next_task(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index e2b2c87f4..549412cdb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -233,6 +233,16 @@ class TaskManager : public std::enable_shared_from_this const std::string& task_id, std::vector labels); + /// This should only be triggered by RobotContext::set_commission(~), and only + /// in scenarios where the idle behavior commission has been toggled off. + void _cancel_idle_behavior(std::vector labels); + + /// Begin the next task for this robot if there is a new task ready to perform + /// and the robot is not already performing a task or caught in an emergency or + /// interruption. If no task is being performed and no new task is ready, the + /// idle behavior will be triggered. + void _begin_next_task(); + private: TaskManager( @@ -408,9 +418,6 @@ class TaskManager : public std::enable_shared_from_this // Map task_id to task_log.json for all tasks managed by this TaskManager std::unordered_map _task_logs = {}; - /// Callback for task timer which begins next task if its deployment time has passed - void _begin_next_task(); - /// Begin performing an emergency pullover. This should only be called when an /// emergency is active. void _begin_pullover(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 2faa8340f..5365b922f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -17,6 +17,7 @@ #include "RobotContext.hpp" #include "internal_RobotUpdateHandle.hpp" +#include "../TaskManager.hpp" #include #include @@ -973,8 +974,25 @@ std::shared_ptr RobotContext::task_manager() //============================================================================== void RobotContext::set_commission(RobotUpdateHandle::Commission value) { - std::lock_guard lock(*_commission_mutex); - _commission = std::move(value); + { + std::lock_guard lock(*_commission_mutex); + _commission = std::move(value); + } + + if (const auto mgr = _task_manager.lock()) + { + if (!_commission.is_performing_idle_behavior()) + { + mgr->_cancel_idle_behavior({"decommissioned"}); + } + else + { + // We trigger this in case the robot needs to begin its idle behavior. + // If it shouldn't perform idle behavior for any reason (e.g. already + // performing a task), then this will have no effect. + mgr->_begin_next_task(); + } + } } //============================================================================== From 5494dec8cfe151677607d1c7d90bec60c1b8cc6d Mon Sep 17 00:00:00 2001 From: Xiyu Date: Mon, 6 May 2024 20:20:30 +0800 Subject: [PATCH 052/119] Add fleet-level reassign dispatched tasks API (#348) Signed-off-by: Xiyu Oh Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/FleetUpdateHandle.hpp | 3 +++ .../rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 14 ++++++++++++++ rmf_fleet_adapter_python/src/adapter.cpp | 4 +++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 4c536bd98..9a4c4d9d4 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -401,6 +401,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this FleetUpdateHandle& set_retreat_to_charger_interval( std::optional duration); + /// Trigger a replan for task allocation for robots in this fleet. + void reassign_dispatched_tasks(); + /// Get the rclcpp::Node that this fleet update handle will be using for /// communication. std::shared_ptr node(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index f121e300c..72d34304a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -2538,6 +2538,20 @@ FleetUpdateHandle& FleetUpdateHandle::set_retreat_to_charger_interval( return *this; } +//============================================================================== +void FleetUpdateHandle::reassign_dispatched_tasks() +{ + _pimpl->worker.schedule( + [w = weak_from_this()](const auto&) + { + const auto self = w.lock(); + if (!self) + return; + self->_pimpl->reassign_dispatched_tasks([]() {}, [](auto) {}); + } + ); +} + //============================================================================== std::shared_ptr FleetUpdateHandle::node() { diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 512710f43..fa610273e 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -596,7 +596,9 @@ PYBIND11_MODULE(rmf_adapter, m) { ); }, py::arg("category"), - py::arg("consider")); + py::arg("consider")) + .def("reassign_dispatched_tasks", + &agv::FleetUpdateHandle::reassign_dispatched_tasks); // TASK REQUEST CONFIRMATION =============================================== auto m_fleet_update_handle = m.def_submodule("fleet_update_handle"); From 908e00ddfc30bf678ca4be23e2582357e2138390 Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 8 May 2024 01:28:10 +0800 Subject: [PATCH 053/119] Fix schema dictionary used during robot status override (#349) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../agv/RobotUpdateHandle.cpp | 66 +++++++++++-------- .../agv/internal_RobotUpdateHandle.hpp | 15 +---- 2 files changed, 41 insertions(+), 40 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 68a46eca0..dba26ffab 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -279,6 +279,44 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc) } } +//============================================================================== +std::function +make_schema_loader(const rclcpp::Node::SharedPtr& node) +{ + // Initialize schema dictionary + const std::vector schemas = { + rmf_api_msgs::schemas::robot_state, + rmf_api_msgs::schemas::location_2D, + rmf_api_msgs::schemas::commission, + }; + + std::unordered_map schema_dictionary; + + for (const auto& schema : schemas) + { + const auto json_uri = nlohmann::json_uri{schema["$id"]}; + schema_dictionary.insert({json_uri.url(), schema}); + } + + return [schema_dictionary = std::move(schema_dictionary), node]( + const nlohmann::json_uri& id, + nlohmann::json& value) + { + const auto it = schema_dictionary.find(id.url()); + if (it == schema_dictionary.end()) + { + RCLCPP_ERROR( + node->get_logger(), + "url: %s not found in schema dictionary. " + "Status for robot will not be overwritten.", + id.url().c_str()); + return; + } + + value = it->second; + }; +} + //============================================================================== void RobotUpdateHandle::override_status(std::optional status) { @@ -286,40 +324,16 @@ void RobotUpdateHandle::override_status(std::optional status) { if (status.has_value()) { - // Here we capture [this] to avoid potential costly copy of - // schema_dictionary when more enties are inserted in the future. - // It is permissible here since the lambda will only be used within the - // scope of this function. - const auto loader = - [context, this]( - const nlohmann::json_uri& id, - nlohmann::json& value) - { - const auto it = _pimpl->schema_dictionary.find(id.url()); - if (it == _pimpl->schema_dictionary.end()) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "url: %s not found in schema dictionary. " - "Status for robot [%s] will not be overwritten.", - id.url().c_str(), - context->name().c_str()); - return; - } - - value = it->second; - }; - try { static const auto validator = nlohmann::json_schema::json_validator( - rmf_api_msgs::schemas::robot_state, loader); + rmf_api_msgs::schemas::robot_state, + make_schema_loader(context->node())); nlohmann::json dummy_msg; dummy_msg["status"] = status.value(); validator.validate(dummy_msg); - } catch (const std::exception& e) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index 7e6bc133d..5d3f7886d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include @@ -261,8 +262,6 @@ class RobotUpdateHandle::Implementation std::string name; RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::Unstable(); bool reported_loss = false; - std::unordered_map schema_dictionary = {}; - static std::shared_ptr make(RobotContextPtr context) { @@ -274,18 +273,6 @@ class RobotUpdateHandle::Implementation handle._pimpl->unstable._pimpl = &RobotUpdateHandle::Implementation::get(handle); - // Initialize schema dictionary - const std::vector schemas = { - rmf_api_msgs::schemas::robot_state, - rmf_api_msgs::schemas::location_2D, - }; - - for (const auto& schema : schemas) - { - const auto json_uri = nlohmann::json_uri{schema["$id"]}; - handle._pimpl->schema_dictionary.insert({json_uri.url(), schema}); - } - return std::make_shared(std::move(handle)); } From eaec283e018be63d294f167621f1dd0f8093b881 Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 10 May 2024 18:38:18 +0800 Subject: [PATCH 054/119] Filter DoorOpen insertion by map name (#353) Signed-off-by: Michael X. Grey Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/events/ExecutePlan.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 7887629ad..7c727de31 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -740,13 +740,18 @@ std::optional ExecutePlan::make( if (first_graph_wp.has_value()) { - const Eigen::Vector2d p1 = - graph.get_waypoint(*first_graph_wp).get_location(); + const auto& wp = graph.get_waypoint(*first_graph_wp); + const Eigen::Vector2d p1 = wp.get_location(); + const auto& map = wp.get_map_name(); // Check if the line from the start of the plan to this waypoint crosses // through a door, and add a DoorOpen phase if it does for (const auto& door : graph.all_known_doors()) { + if (door->map() != map) + { + continue; + } if (door->intersects(p0, p1, envelope)) { @@ -763,7 +768,6 @@ std::optional ExecutePlan::make( } } - const auto& map = graph.get_waypoint(*first_graph_wp).get_map_name(); // Check if the robot is going into a lift and summon the lift for (const auto& lift : graph.all_known_lifts()) { From dba5bbd23fe5038f2d5f77f3e0133c0477f8696e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Sat, 11 May 2024 02:39:10 +0800 Subject: [PATCH 055/119] Event based lift / door logic (#320) Signed-off-by: Luca Della Vedova Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/door_supervisor/Node.cpp | 3 ++- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 10 +++++++--- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 6 ++++++ 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/door_supervisor/Node.cpp b/rmf_fleet_adapter/src/door_supervisor/Node.cpp index 5b5f149af..83abfc929 100644 --- a/rmf_fleet_adapter/src/door_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/door_supervisor/Node.cpp @@ -28,7 +28,8 @@ const std::string DoorSupervisorRequesterID = "door_supervisor"; Node::Node() : rclcpp::Node("door_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + const auto default_qos = + rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable(); _door_request_pub = create_publisher( FinalDoorRequestTopicName, default_qos); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index e1bb36e60..de59217ab 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -27,7 +27,8 @@ namespace lift_supervisor { Node::Node() : rclcpp::Node("rmf_lift_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + const auto default_qos = + rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable(); const auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); @@ -68,12 +69,14 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) { if (curr_request->session_id == msg->session_id) { + msg->request_time = this->now(); + _lift_request_pub->publish(*msg); if (msg->request_type != LiftRequest::REQUEST_END_SESSION) + { curr_request = std::move(msg); + } else { - msg->request_time = this->now(); - _lift_request_pub->publish(*msg); RCLCPP_INFO( this->get_logger(), "[%s] Published end lift session from lift supervisor", @@ -85,6 +88,7 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) } else { + _lift_request_pub->publish(*msg); if (msg->request_type != LiftRequest::REQUEST_END_SESSION) { curr_request = std::move(msg); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 5365b922f..a3a31f63b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1077,6 +1077,12 @@ void RobotContext::release_lift() "Releasing lift [%s] for [%s]", _lift_destination->lift_name.c_str(), requester_id().c_str()); + rmf_lift_msgs::msg::LiftRequest msg; + msg.lift_name = _lift_destination->lift_name; + msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; + msg.session_id = requester_id(); + msg.destination_floor = _lift_destination->destination_floor; + _node->lift_request()->publish(msg); } _lift_destination = nullptr; _initial_time_idle_outside_lift = std::nullopt; From 8df5babc8fe02ebf8e8f262d0747e6d3eb0bfa14 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 7 Jun 2024 16:27:07 +0800 Subject: [PATCH 056/119] Fix segfault Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index d615a5c28..11538b598 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -870,7 +870,8 @@ void GoToPlace::Active::_stop_and_clear() if (const auto command = _context->command()) command->stop(); - _retry_timer->cancel(); + if (_retry_timer) + _retry_timer->cancel(); _context->itinerary().clear(); } From 550c81708e05bc35bb0e25a20bed5d22cc990588 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 10 Jun 2024 12:33:47 +0800 Subject: [PATCH 057/119] Fix compile issues. Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 4 ---- .../src/rmf_fleet_adapter/events/EmergencyPullover.cpp | 2 +- .../src/rmf_fleet_adapter/events/GoToPlace.hpp | 9 +++++++++ 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 0d9a30f21..f5e4187ba 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -940,10 +940,6 @@ class RobotContext _mutex_group_manual_release_sub; std::chrono::steady_clock::time_point _last_active_task_time; - rclcpp::Subscription::SharedPtr - _mutex_group_manual_release_sub; - std::chrono::steady_clock::time_point _last_active_task_time; - uint64_t _last_reservation_request_id; ReservationManager _reservation_mgr; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 222df2d6f..b87759a7c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -243,7 +243,7 @@ void EmergencyPullover::Active::_find_plan() _state->update_log().info("Searching for an emergency pullover"); _find_pullover_service = std::make_shared( - _context->emergency_planner(), _context->location(), + _context->emergency_planner(), _context->_get_free_spots(), _context->location(), _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 7c10c9382..2d9e1c65f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -105,6 +105,15 @@ class GoToPlace : public rmf_task_sequence::Event void kill() final; private: + enum class ReservationState + { + Pending=0, + Requested=1, + RecievedResponse=2 + }; + + ReservationState _current_reservation_state = ReservationState::Pending; + Active(Description description); From 1c89d26e5dfd7dd1de83eaf6820766a0549bb2ab Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 19 Jul 2024 10:42:33 +0800 Subject: [PATCH 058/119] Get config from yaml to `RobotContext` Signed-off-by: Arjo Chakravarty --- rmf_chope_node/package.xml | 6 +-- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 16 ++++++- .../agv/RobotUpdateHandle.hpp | 4 ++ .../src/rmf_fleet_adapter/agv/Adapter.cpp | 3 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 47 ++++++++++++++++--- .../rmf_fleet_adapter/agv/RobotContext.cpp | 12 +++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 8 ++++ .../agv/RobotUpdateHandle.cpp | 14 ++++++ .../agv/internal_EasyFullControl.hpp | 5 +- 9 files changed, 103 insertions(+), 12 deletions(-) diff --git a/rmf_chope_node/package.xml b/rmf_chope_node/package.xml index e76c50306..06425cc46 100644 --- a/rmf_chope_node/package.xml +++ b/rmf_chope_node/package.xml @@ -3,9 +3,9 @@ rmf_chope_node 0.0.0 - TODO: Package description - arjoc - TODO: License declaration + Node that handles current state of parking spots. + Arjo Chakravarty + Apache License 2.0 ament_cmake diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index c6eb2cd30..354ec2a4f 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -255,6 +255,9 @@ class EasyFullControl::RobotConfiguration /// \param[in] responsive_wait /// Should this robot use the responsive wait behavior? true / false / fleet default. /// + /// \param[in] use_parking_spot + /// Use parking spot system. + /// /// \warning This must contain a single string value until a later release of /// RMF. We are using a vector for forward API compatibility. For now, make /// sure each robot has only one unique compatible charger to avoid charging @@ -262,6 +265,7 @@ class EasyFullControl::RobotConfiguration RobotConfiguration( std::vector compatible_chargers, std::optional responsive_wait = std::nullopt, + bool use_parking_spot_system = false, std::optional max_merge_waypoint_distance = 1e-3, std::optional max_merge_lane_distance = 0.3, std::optional min_lane_length = 1e-8); @@ -281,6 +285,9 @@ class EasyFullControl::RobotConfiguration /// be used. std::optional responsive_wait() const; + /// Check if the robot should use the parking spot system. + bool use_parking_spot_system() const; + /// Toggle responsive wait on (true), off (false), or use fleet default /// (std::nullopt). void set_responsive_wait(std::optional enable); @@ -559,6 +566,9 @@ class EasyFullControl::FleetConfiguration /// /// \param[in] default_min_lane_length /// The minimum length that a lane should have. + /// + /// \param[in] use_parking_reservation + /// Whether ro use FleetConfiguration( const std::string& fleet_name, std::optional> @@ -585,7 +595,8 @@ class EasyFullControl::FleetConfiguration bool default_responsive_wait = false, double default_max_merge_waypoint_distance = 1e-3, double default_max_merge_lane_distance = 0.3, - double min_lane_length = 1e-8 + double min_lane_length = 1e-8, + bool use_parking_reservation = false ); /// Create a FleetConfiguration object using a set of configuration parameters @@ -767,6 +778,9 @@ class EasyFullControl::FleetConfiguration /// Should robots in this fleet have responsive wait enabled by default? bool default_responsive_wait() const; + /// Should robots use the parking reservation system. + bool use_parking_reservation_system() const; + /// Set whether robots in this fleet should have responsive wait enabled by /// default. void set_default_responsive_wait(bool enable); diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 132c749c6..23f62dd45 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -408,6 +408,10 @@ class RobotUpdateHandle /// By default this behavior is enabled. void enable_responsive_wait(bool value); + + /// Enable simple parking spot + void enable_simple_parkingspot_reservation(bool value); + /// If the robot is holding onto a session with a lift, release that session. void release_lift(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index 9c1e4d5dc..9db238ea0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -382,7 +382,8 @@ std::shared_ptr Adapter::add_easy_fleet( config.default_responsive_wait(), config.default_max_merge_waypoint_distance(), config.default_max_merge_lane_distance(), - config.default_min_lane_length()); + config.default_min_lane_length(), + config.use_parking_reservation_system()); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index b44238d45..5da1a9672 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -122,12 +122,14 @@ class EasyFullControl::RobotConfiguration::Implementation std::optional max_merge_waypoint_distance; std::optional max_merge_lane_distance; std::optional min_lane_length; + bool use_parking_spot; }; //============================================================================== EasyFullControl::RobotConfiguration::RobotConfiguration( std::vector compatible_chargers, std::optional responsive_wait, + bool use_parking_spot_system, std::optional max_merge_waypoint_distance, std::optional max_merge_lane_distance, std::optional min_lane_length) @@ -136,7 +138,8 @@ EasyFullControl::RobotConfiguration::RobotConfiguration( responsive_wait, max_merge_waypoint_distance, max_merge_lane_distance, - min_lane_length + min_lane_length, + use_parking_spot_system })) { // Do nothing @@ -162,6 +165,12 @@ std::optional EasyFullControl::RobotConfiguration::responsive_wait() const return _pimpl->responsive_wait; } +//============================================================================== +bool EasyFullControl::RobotConfiguration::use_parking_spot_system() const +{ + return _pimpl->use_parking_spot; +} + //============================================================================== void EasyFullControl::RobotConfiguration::set_responsive_wait( std::optional enable) @@ -1702,6 +1711,7 @@ class EasyFullControl::FleetConfiguration::Implementation double default_max_merge_lane_distance; double default_min_lane_length; std::unordered_map lift_emergency_levels; + bool use_parking_reservation; }; //============================================================================== @@ -1730,7 +1740,8 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( bool default_responsive_wait, double default_max_merge_waypoint_distance, double default_max_merge_lane_distance, - double default_min_lane_length) + double default_min_lane_length, + bool use_parking_reservation) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(fleet_name), @@ -1757,7 +1768,8 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(default_max_merge_waypoint_distance), std::move(default_max_merge_lane_distance), std::move(default_min_lane_length), - {} + {}, + std::move(use_parking_reservation) })) { // Do nothing @@ -2202,6 +2214,14 @@ EasyFullControl::FleetConfiguration::from_config_files( default_responsive_wait = default_responsive_wait_yaml.as(); } + bool use_simple_parking_reservation_system = false; + const YAML::Node& parking_reservation = rmf_fleet["use_parking_reservations"]; + if (parking_reservation) + { + use_simple_parking_reservation_system = + parking_reservation.as(); + } + double default_max_merge_waypoint_distance = 1e-3; const YAML::Node& default_max_merge_waypoint_distance_yaml = rmf_fleet["max_merge_waypoint_distance"]; @@ -2300,6 +2320,7 @@ EasyFullControl::FleetConfiguration::from_config_files( auto config = RobotConfiguration( std::move(chargers), responsive_wait, + use_simple_parking_reservation_system, max_merge_waypoint_distance, max_merge_lane_distance, min_lane_length); @@ -2361,7 +2382,8 @@ EasyFullControl::FleetConfiguration::from_config_files( default_responsive_wait, default_max_merge_waypoint_distance, default_max_merge_lane_distance, - default_min_lane_length); + default_min_lane_length, + use_simple_parking_reservation_system); config.change_lift_emergency_levels() = lift_emergency_levels; config.set_retreat_to_charger_interval(retreat_to_charger_interval); return config; @@ -2676,6 +2698,13 @@ bool EasyFullControl::FleetConfiguration::default_responsive_wait() const return _pimpl->default_responsive_wait; } +//============================================================================== +bool EasyFullControl::FleetConfiguration::use_parking_reservation_system() +const +{ + return _pimpl->use_parking_reservation; +} + //============================================================================== void EasyFullControl::FleetConfiguration::set_default_responsive_wait( bool enable) @@ -2944,6 +2973,8 @@ auto EasyFullControl::add_robot( enable_responsive_wait = *configuration.responsive_wait(); } + bool enable_parking_spot = configuration.use_parking_spot_system(); + _pimpl->fleet_handle->add_robot( insertion.first->second, robot_name, @@ -2959,7 +2990,8 @@ auto EasyFullControl::add_robot( action_executor = callbacks.action_executor(), localization = std::move(localization), nav_params = robot_nav_params, - enable_responsive_wait + enable_responsive_wait, + enable_parking_spot ](const RobotUpdateHandlePtr& updater) { auto context = RobotUpdateHandle::Implementation::get(*updater) @@ -2978,7 +3010,8 @@ auto EasyFullControl::add_robot( localization, context, nav_params, - enable_responsive_wait + enable_responsive_wait, + enable_parking_spot ](const auto&) { cmd_handle->w_context = context; @@ -2993,6 +3026,8 @@ auto EasyFullControl::add_robot( } handle->enable_responsive_wait(enable_responsive_wait); + context->_set_parking_spot_manager(enable_parking_spot); + RCLCPP_INFO( node->get_logger(), "Successfully added robot [%s] to the fleet [%s].", diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 577a6dfbd..191f9a598 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1677,6 +1677,18 @@ void RobotContext::_set_allocated_destination( _reservation_mgr.add_ticket(ticket); } +//============================================================================== +void RobotContext::_set_parking_spot_manager(const bool enabled) +{ + this->use_parking_spot_reservations = enabled; +} + +//============================================================================== +bool RobotContext::_parking_spot_manager_enabled() +{ + return this->use_parking_spot_reservations; +} + //============================================================================== std::optional RobotContext::_release_resource() diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 10d9b733f..481cdbff8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -758,6 +758,12 @@ class RobotContext void _set_allocated_destination( const rmf_chope_msgs::msg::ReservationAllocation&); + /// Set if the parking spot manager is used or not + void _set_parking_spot_manager(const bool enabled); + + /// Set if the parking spot manager is used or not + bool _parking_spot_manager_enabled(); + /// Release last resource that was acquired. std::optional _release_resource(); @@ -969,6 +975,8 @@ class RobotContext std::optional _final_lift_destination; std::unique_ptr _final_lift_destination_mutex = std::make_unique(); + + bool use_parking_spot_reservations; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 761d11216..2c9ffa02d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -712,6 +712,20 @@ void RobotUpdateHandle::enable_responsive_wait(bool value) }); } +//============================================================================== +void RobotUpdateHandle::enable_simple_parkingspot_reservation(bool value) +{ + const auto context = _pimpl->get_context(); + if (!context) + return; + + context->worker().schedule( + [context, value](const auto&) + { + context->_set_parking_spot_manager(value); + }); +} + //============================================================================== void RobotUpdateHandle::release_lift() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp index f56291c12..64014cebb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp @@ -36,12 +36,14 @@ class EasyFullControl::Implementation std::unordered_map cmd_handles; NavParams nav_params; bool default_responsive_wait; + bool use_parking_reservation_node; static std::shared_ptr make( std::shared_ptr fleet_handle, bool skip_rotation_commands, std::shared_ptr transforms_to_robot_coords, bool default_responsive_wait, + bool use_parking_reservation_node, double default_max_merge_waypoint_distance, double default_max_merge_lane_distance, double default_min_lane_length) @@ -58,7 +60,8 @@ class EasyFullControl::Implementation default_max_merge_lane_distance, default_min_lane_length }, - default_responsive_wait + default_responsive_wait, + use_parking_reservation_node }); return handle; } From ad92262e0259c5beac77896ab159e1555d64c59d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 19 Jul 2024 12:22:54 +0800 Subject: [PATCH 059/119] Restore old behaviour by default. Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 175 ++++++++++++------ 1 file changed, 117 insertions(+), 58 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 11538b598..e509bf14e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -542,23 +542,121 @@ std::optional GoToPlace::Active::_choose_goal( _context->requester_id().c_str()); - // No need to use reservation system if we are already there. - if (_description.one_of().size() == 1 - && _description.one_of()[0].waypoint() == current_location[0].waypoint() - && _context->_has_ticket()) + if (_context->_parking_spot_manager_enabled()) { - return _description.one_of()[0]; - } + RCLCPP_INFO(_context->node()->get_logger(), "Make"); + // No need to use reservation system if we are already there. + if (_description.one_of().size() == 1 + && _description.one_of()[0].waypoint() == current_location[0].waypoint() + && _context->_has_ticket()) + { + return _description.one_of()[0]; + } + if (_current_reservation_state == ReservationState::Pending) + { + // Select node + rmf_chope_msgs::msg::FlexibleTimeRequest ftr; + ftr.header.robot_name = _context->name(); + ftr.header.fleet_name = _context->group(); + ftr.header.request_id = _reservation_id; + + auto lowest_cost = std::numeric_limits::infinity(); + std::optional selected_idx; + for (std::size_t i = 0; i < _description.one_of().size(); ++i) + { + const auto wp_idx = _description.one_of()[i].waypoint(); + if (only_same_map) + { + const auto& wp = graph.get_waypoint(wp_idx); - if (_current_reservation_state == ReservationState::Pending) - { - // Select node - rmf_chope_msgs::msg::FlexibleTimeRequest ftr; - ftr.header.robot_name = _context->name(); - ftr.header.fleet_name = _context->group(); - ftr.header.request_id = _reservation_id; + // Check if same map. If not don't consider location. This is to ensure + // the robot does not try to board a lift. + if (wp.get_map_name() != _context->map()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + _context->map().c_str()); + continue; + } + } + + // Find distance to said point + auto result = + _context->planner()->quickest_path(current_location, wp_idx); + if (result.has_value()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Got distance from [%lu] as %f", + wp_idx, + result->cost()); + + if (result->cost() < lowest_cost) + { + selected_idx = i; + lowest_cost = result->cost(); + } + + std::stringstream json_stream; + json_stream << wp_idx << std::endl; + std::string json; + json_stream >> json; + + rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; + alternative.resource_name = json; + alternative.cost = result->cost(); + alternative.has_end = false; + rmf_chope_msgs::msg::StartTimeRange start; + start.earliest_start_time = _context->node()->get_clock()->now(); + start.latest_start_time = start.earliest_start_time; + start.has_earliest_start_time = true; + start.has_latest_start_time = true; + alternative.start_time = start; + + ftr.alternatives.push_back(alternative); + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + wp_idx, + _context->requester_id().c_str()); + } + } + _context->node()->location_requester()->publish(ftr); + _current_reservation_state = ReservationState::Requested; + } + else if (_current_reservation_state == ReservationState::RecievedResponse) + { + // Go to recommended destination + if (_final_allocated_destination.value()->instruction_type == + rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + { + _context->_set_allocated_destination( + *_final_allocated_destination.value().get()); + return _description.one_of()[_final_allocated_destination.value()-> + satisfies_alternative]; + } + // For queueing system in future + //return rmf_traffic::agv::Plan::Goal(std::stoul(_final_allocated_destination.value()->resource), std::nullopt); + + // For now just keep retrying until your turn. + // Probably not needed since, the node does not publish an allocation till its available? + RCLCPP_ERROR( + _context->node()->get_logger(), + "Unable to immediately service call, awaiting more." + ); + return std::nullopt; + } + } + else + { auto lowest_cost = std::numeric_limits::infinity(); std::optional selected_idx; for (std::size_t i = 0; i < _description.one_of().size(); ++i) @@ -583,8 +681,7 @@ std::optional GoToPlace::Active::_choose_goal( } // Find distance to said point - auto result = - _context->planner()->quickest_path(current_location, wp_idx); + auto result = _context->planner()->quickest_path(current_location, wp_idx); if (result.has_value()) { RCLCPP_INFO( @@ -598,59 +695,21 @@ std::optional GoToPlace::Active::_choose_goal( selected_idx = i; lowest_cost = result->cost(); } - - std::stringstream json_stream; - json_stream << wp_idx << std::endl; - std::string json; - json_stream >> json; - - rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; - alternative.resource_name = json; - alternative.cost = result->cost(); - alternative.has_end = false; - - rmf_chope_msgs::msg::StartTimeRange start; - start.earliest_start_time = _context->node()->get_clock()->now(); - start.latest_start_time = start.earliest_start_time; - start.has_earliest_start_time = true; - start.has_latest_start_time = true; - alternative.start_time = start; - - ftr.alternatives.push_back(alternative); } else { RCLCPP_ERROR( _context->node()->get_logger(), "No path found for robot [%s] to waypoint [%lu]", - wp_idx, - _context->requester_id().c_str()); + _context->requester_id().c_str(), + wp_idx); } } - _context->node()->location_requester()->publish(ftr); - _current_reservation_state = ReservationState::Requested; - } - else if (_current_reservation_state == ReservationState::RecievedResponse) - { - // Go to recommended destination - if (_final_allocated_destination.value()->instruction_type == - rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + + if (selected_idx.has_value()) { - _context->_set_allocated_destination( - *_final_allocated_destination.value().get()); - return _description.one_of()[_final_allocated_destination.value()-> - satisfies_alternative]; + return _description.one_of()[*selected_idx]; } - // For queueing system in future - //return rmf_traffic::agv::Plan::Goal(std::stoul(_final_allocated_destination.value()->resource), std::nullopt); - - // For now just keep retrying until your turn. - // Probably not needed since, the node does not publish an allocation till its available? - RCLCPP_ERROR( - _context->node()->get_logger(), - "Unable to immediately service call, awaiting more." - ); - return std::nullopt; } return std::nullopt; From b8f0100fd8a5e44398124f149324226338888f3a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 19 Jul 2024 17:50:13 +0800 Subject: [PATCH 060/119] Only proceed if next spot is free. Note: Things are still broken Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 23 +++++++++++++------ 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index e509bf14e..2c870ee14 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -258,8 +258,7 @@ auto GoToPlace::Active::make( active->_context->worker())) .subscribe([w = active->weak_from_this()](const std::shared_ptr - & - msg) + &msg) { const auto self = w.lock(); if (!self) @@ -276,9 +275,13 @@ auto GoToPlace::Active::make( } self->_final_allocated_destination = msg; - self->_current_reservation_state = ReservationState::RecievedResponse; - self->_retry_timer->cancel(); - self->_find_plan(); + if (msg->instruction_type + == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + { + self->_current_reservation_state = ReservationState::RecievedResponse; + self->_retry_timer->cancel(); + self->_find_plan(); + } }); @@ -544,7 +547,8 @@ std::optional GoToPlace::Active::_choose_goal( if (_context->_parking_spot_manager_enabled()) { - RCLCPP_INFO(_context->node()->get_logger(), "Make"); + RCLCPP_INFO(_context->node()->get_logger(), + "Requesting Chope Node For Time To Progress"); // No need to use reservation system if we are already there. if (_description.one_of().size() == 1 && _description.one_of()[0].waypoint() == current_location[0].waypoint() @@ -638,6 +642,10 @@ std::optional GoToPlace::Active::_choose_goal( if (_final_allocated_destination.value()->instruction_type == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Recieved proceed to final destination from chope node %d: ", + _final_allocated_destination.value()); _context->_set_allocated_destination( *_final_allocated_destination.value().get()); return _description.one_of()[_final_allocated_destination.value()-> @@ -650,13 +658,14 @@ std::optional GoToPlace::Active::_choose_goal( // Probably not needed since, the node does not publish an allocation till its available? RCLCPP_ERROR( _context->node()->get_logger(), - "Unable to immediately service call, awaiting more." + "Chope node said its unable to immediately service call, awaiting more." ); return std::nullopt; } } else { + RCLCPP_INFO(_context->node()->get_logger(), "Skipping chope node."); auto lowest_cost = std::numeric_limits::infinity(); std::optional selected_idx; for (std::size_t i = 0; i < _description.one_of().size(); ++i) From 82176af95e6f99f60f049e6ce6f711a9c6225ed5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 23 Jul 2024 11:25:57 -0400 Subject: [PATCH 061/119] Still broken: Fixing implementation of wait and spot and then go behaviour Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 179 ++++++++++++++++++++++++++++++++---- 1 file changed, 159 insertions(+), 20 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 8271eb2a6..ce816f23e 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -135,9 +135,10 @@ struct LocationReq } }; + /// Implements a simple Mutex. Only one robot can claim a location at a time. -/// The current implementation is relatively simplistic and basically checks if a location is occupied or not. -/// A queuing system is in the works. Note: It is possible for the current system to get deadlocked. +/// The current implementation is relatively simplistic and basically checks +/// if a location is occupied or not. A queuing system is in the works. class CurrentState { public: @@ -171,7 +172,9 @@ class CurrentState { if (_ticket_to_location.count(ticket_id) != 0) { - // Ticket has been allocated. Probably some DDS-ism causing the issue + /// This should never be reached. If this is reached it means that some + /// client fleetadapter node is misbehaving and requesting the same ticket + /// twice. std::cerr << "Ticket already allocated" << std::endl; return std::nullopt; } @@ -206,17 +209,17 @@ class CurrentState return std::nullopt; } - bool release(const std::size_t ticket_id) + std::optional release(const std::size_t ticket_id) { std::lock_guard lock(_mtx); auto _ticket = _ticket_to_location.find(ticket_id); if (_ticket == _ticket_to_location.end()) { - return false; + return std::nullopt; } auto location = _ticket->second; _current_location_reservations[location].ticket = std::nullopt; - return true; + return {location}; } private: @@ -225,12 +228,82 @@ class CurrentState std::unordered_map _ticket_to_location; }; + +template +class ItemQueue +{ +public: + void add(T item) + { + index_to_item[curr_index] = item; + item_to_index[item] = curr_index; + indices.insert(curr_index); + curr_index++; + } + + // Log(n) + void remove_item(T item) + { + auto index = item_to_index[item]; + item_to_index.erase(item); + index_to_item.erase(index); + indices.erase(index); + } + + + std::optional front() + { + auto it = indices.begin(); + if (it == indices.end()) + { + return std::nullopt; + } + return index_to_item[*it]; + } + + std::size_t curr_index = 0; + + std::unordered_map index_to_item; + std::unordered_map item_to_index; + std::set indices; +}; + +/// This class enqueues items based on how old a request is. +class ServiceQueueManager +{ + std::unordered_map> resource_queues; +public: + std::optional service_next_in_queue(const std::string& resource) + { + auto item = resource_queues[resource].front(); + if (!item.has_value()) + { + return std::nullopt; + } + + for (auto& [_, resource_queue]: resource_queues) + { + resource_queue.remove_item(item.value()); + } + return item; + } + + void add_to_queue(std::size_t ticket, std::vector& resources) + { + for(auto resource: resources) + { + resource_queues[resource].add(ticket); + } + } +}; + using namespace std::chrono_literals; -class SimpleQueueSystemNode : public rclcpp::Node +class ChopeNode : public rclcpp::Node { public: - SimpleQueueSystemNode() + ChopeNode() : Node("rmf_chope_node") { @@ -242,21 +315,21 @@ class SimpleQueueSystemNode : public rclcpp::Node request_subscription_ = this->create_subscription( ReservationRequestTopicName, qos, - std::bind(&SimpleQueueSystemNode::on_request, this, + std::bind(&ChopeNode::on_request, this, std::placeholders::_1)); claim_subscription_ = this->create_subscription( ReservationClaimTopicName, qos, - std::bind(&SimpleQueueSystemNode::claim_request, this, + std::bind(&ChopeNode::claim_request, this, std::placeholders::_1)); release_subscription_ = this->create_subscription( ReservationReleaseTopicName, qos, - std::bind(&SimpleQueueSystemNode::release, this, std::placeholders::_1)); + std::bind(&ChopeNode::release, this, std::placeholders::_1)); graph_subscription_ = this->create_subscription( "/nav_graphs", qos, - std::bind(&SimpleQueueSystemNode::recieved_graph, this, + std::bind(&ChopeNode::recieved_graph, this, std::placeholders::_1)); ticket_pub_ = this->create_publisher( @@ -270,7 +343,7 @@ class SimpleQueueSystemNode : public rclcpp::Node timer_ = this->create_wall_timer(500ms, - std::bind(&SimpleQueueSystemNode::publish_free_spots, this)); + std::bind(&ChopeNode::publish_free_spots, this)); } private: @@ -348,6 +421,7 @@ class SimpleQueueSystemNode : public rclcpp::Node rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; allocation.resource = result.value(); + bool within_request = false; for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); i++) @@ -355,11 +429,29 @@ class SimpleQueueSystemNode : public rclcpp::Node if (requests_[request->ticket.ticket_id][i].location == result.value()) { allocation.satisfies_alternative = i; + within_request = true; } } - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - result.value().c_str(), request->ticket.ticket_id); - allocation_pub_->publish(allocation); + if (within_request) + { + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + result.value().c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); + } + else + { + allocation.instruction_type = + rmf_chope_msgs::msg::ReservationAllocation::WAIT_AT_SPOT_AND_THEN_GO; + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + result.value().c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); + std::vector locations; + for(auto loc: requests_[request->ticket.ticket_id]) + { + locations.push_back(loc.location); + } + queue_manager_.add_to_queue(request->ticket.ticket_id, locations); + } } else { @@ -376,12 +468,58 @@ class SimpleQueueSystemNode : public rclcpp::Node this->get_logger(), "Releasing ticket for %lu", request->ticket.ticket_id); auto ticket = request->ticket.ticket_id; - auto success = current_state_.release(ticket); - if (!success) + auto released_location = current_state_.release(ticket); + if (!released_location.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Could not find ticker %lu", + this->get_logger(), "Could not find ticket %lu", request->ticket.ticket_id); + return; + } + + auto next_item = queue_manager_.service_next_in_queue(released_location.value()); + if (next_item.has_value()) + { + // Will go to lowest + auto result = current_state_.allocate_lowest_cost_free_spot(locations, + next_item.value()); + + if (!result.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), "FATAL ERROR: WE SHOULD NEVER GET HERE!!"); + return; + } + rmf_chope_msgs::msg::ReservationAllocation allocation; + bool within_request = false; + for (std::size_t i = 0; i < requests_[next_item].size(); + i++) + { + if (requests_[next_item][i].location == result.value()) + { + allocation.satisfies_alternative = i; + within_request = true; + } + } + if (within_request) + { + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + result.value().c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); + } + else + { + RCLCPP_ERROR( + this->get_logger(), "FATAL ERROR: WE SHOULD NEVER GET HERE!!"); + return; + } + allocation.ticket = ticket_store_.get_existing_ticket( + request->ticket.ticket_id); + allocation.instruction_type = + rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + result.value().c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); } } @@ -412,6 +550,7 @@ class SimpleQueueSystemNode : public rclcpp::Node std::unordered_map> requests_; TicketStore ticket_store_; CurrentState current_state_; + ServiceQueueManager queue_manager_; rclcpp::TimerBase::SharedPtr timer_; }; @@ -419,7 +558,7 @@ class SimpleQueueSystemNode : public rclcpp::Node int main(int argc, const char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file From a51157f174e93460baaed1aa421355219b73677f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Jul 2024 07:36:07 -0400 Subject: [PATCH 062/119] Fix compilation eror Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index ce816f23e..cb1041712 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -481,7 +481,7 @@ class ChopeNode : public rclcpp::Node if (next_item.has_value()) { // Will go to lowest - auto result = current_state_.allocate_lowest_cost_free_spot(locations, + auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_item.value()], next_item.value()); if (!result.has_value()) @@ -492,10 +492,10 @@ class ChopeNode : public rclcpp::Node } rmf_chope_msgs::msg::ReservationAllocation allocation; bool within_request = false; - for (std::size_t i = 0; i < requests_[next_item].size(); + for (std::size_t i = 0; i < requests_[next_item.value()].size(); i++) { - if (requests_[next_item][i].location == result.value()) + if (requests_[next_item.value()][i].location == result.value()) { allocation.satisfies_alternative = i; within_request = true; From 999d04a1f3bab4615ed1529b55cfd9e9cbef0609 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 29 Jul 2024 10:17:32 -0400 Subject: [PATCH 063/119] Giant refactor Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 190 +++++------ .../rmf_fleet_adapter/events/GoToPlace.cpp | 303 ++++------------- .../rmf_fleet_adapter/events/GoToPlace.hpp | 18 +- .../events/internal_ChopeNegotiator.hpp | 319 ++++++++++++++++++ 4 files changed, 493 insertions(+), 337 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index cb1041712..af31039b7 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -74,8 +74,9 @@ struct std::hash }; -/// Ticket generation class for book keeping purposes. Will eventually overflow and leak memory. +/// Ticket generation class for book keeping purposes. Will eventually overflow. /// Ticket id 0 does not exist and is useful for making emergency claims. +/// Ticket ids are mapped across multiple fleets. class TicketStore { @@ -166,7 +167,11 @@ class CurrentState } } - std::optional allocate_lowest_cost_free_spot( + /// Tries to greedily allocate the lowest cost free spot given a list of potential + /// parking spots. + /// \param[in] incoming_requests - Parking spot and cost of said parking spot + /// \param[in] ticket_id - Ticket which is being + std::optional allocate_lowest_cost_free_spot( const std::vector& incoming_requests, const std::size_t ticket_id) { @@ -179,6 +184,12 @@ class CurrentState return std::nullopt; } + std::unordered_map positions; + for (std::size_t i = 0; i < incoming_requests.size(); ++i) + { + positions[incoming_requests[i].location] = i; + } + auto requests = incoming_requests; std::sort(requests.begin(), requests.end()); std::lock_guard lock(_mtx); @@ -187,22 +198,24 @@ class CurrentState auto parking = _current_location_reservations.find(requests[i].location); if (parking == _current_location_reservations.end()) { + // New parking spot not in list. Should be fine to occupy. _current_location_reservations[requests[i].location] = LocationState {ticket_id}; _ticket_to_location.emplace(ticket_id, requests[i].location); - return requests[i].location; + return positions[requests[i].location]; } else if (!parking->second.ticket.has_value()) { + // Existing parking spot. _current_location_reservations[requests[i].location].ticket = ticket_id; _ticket_to_location.emplace(ticket_id, requests[i].location); - return parking->first; + return positions[parking->first]; } } - std::cerr << "Could not find route to any of: "; + std::cerr << "Could not free space from any of: "; for (auto c: requests) { - std::cerr << "" << c.location <<", "; + std::cerr << c.location << ", "; } std::cerr << "\n"; @@ -229,10 +242,13 @@ class CurrentState }; +///A queue that allowss removal of an item based on its value. template class ItemQueue { public: + /// Adds an item to the queue. + /// Time complexity: O(1) void add(T item) { index_to_item[curr_index] = item; @@ -241,16 +257,22 @@ class ItemQueue curr_index++; } + // Removes the item from the queue // Log(n) void remove_item(T item) { - auto index = item_to_index[item]; + auto index = item_to_index.find(item); + if (index == item_to_index.end()) + { + return; + } item_to_index.erase(item); - index_to_item.erase(index); - indices.erase(index); + index_to_item.erase(index->second); + indices.erase(index->second); } - + // Gives the most recent item in the queue. + // Returns nullopt if the queue is empty. std::optional front() { auto it = indices.begin(); @@ -269,6 +291,10 @@ class ItemQueue }; /// This class enqueues items based on how old a request is. +/// The basic idea is that we maintain a queue for every resource. As requests +/// come in we simultaneously add them to every queue which cam beserviced. +/// Once a resource becomes free we call `service_next_in_queue` for said resource. +/// When we service the next item in the queue we remove it from all other queues. class ServiceQueueManager { std::unordered_map locations; - for (auto claim: requests_[request->ticket.ticket_id]) + for (auto location_pref: requests_[request->ticket.ticket_id]) { - locations.push_back(claim); + locations.push_back(location_pref); } - auto cost = (locations.size() == 0) ? 0.0 : locations.back().cost + 1.0; - for (auto claim: request->wait_points) - { - locations.push_back(LocationReq { - claim, - cost - }); - cost += 1.0; - } + // Allocate the lowest cost free spot from list of intended final locations if possible auto result = current_state_.allocate_lowest_cost_free_spot(locations, request->ticket.ticket_id); if (result.has_value()) @@ -419,45 +437,45 @@ class ChopeNode : public rclcpp::Node request->ticket.ticket_id); allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + allocation.satisfies_alternative = result.value(); + allocation.resource = requests_[request->ticket.ticket_id][result.value()].location; - allocation.resource = result.value(); - bool within_request = false; + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + allocation.resource.c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); + return; - for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); - i++) - { - if (requests_[request->ticket.ticket_id][i].location == result.value()) - { - allocation.satisfies_alternative = i; - within_request = true; - } - } - if (within_request) - { - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - result.value().c_str(), request->ticket.ticket_id); - allocation_pub_->publish(allocation); - } - else - { - allocation.instruction_type = - rmf_chope_msgs::msg::ReservationAllocation::WAIT_AT_SPOT_AND_THEN_GO; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - result.value().c_str(), request->ticket.ticket_id); - allocation_pub_->publish(allocation); - std::vector locations; - for(auto loc: requests_[request->ticket.ticket_id]) - { - locations.push_back(loc.location); - } - queue_manager_.add_to_queue(request->ticket.ticket_id, locations); - } } - else + + std::vector wait_points; + auto cost = 0.0; + for (auto waitpoint_name: request->wait_points) + { + + LocationReq request{ + waitpoint_name, + cost + }; + wait_points.push_back(request); + cost += 1.0; + } + + waitpoints_[request->ticket.ticket_id] = wait_points; + auto waitpoint_result = current_state_.allocate_lowest_cost_free_spot(locations, + request->ticket.ticket_id); + if (waitpoint_result.has_value()) { - RCLCPP_INFO( - this->get_logger(), "Could not allocate resource for ticket %lu.", + rmf_chope_msgs::msg::ReservationAllocation allocation; + allocation.ticket = ticket_store_.get_existing_ticket( request->ticket.ticket_id); + allocation.instruction_type = + rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY; + allocation.satisfies_alternative = result.value(); + allocation.resource = wait_points[result.value()].location; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Could not allocate a waiting point for robots"); } } @@ -478,49 +496,30 @@ class ChopeNode : public rclcpp::Node } auto next_item = queue_manager_.service_next_in_queue(released_location.value()); - if (next_item.has_value()) + if (!next_item.has_value()) { - // Will go to lowest - auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_item.value()], - next_item.value()); + return; + } + // Will go to lowest + auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_item.value()], + next_item.value()); - if (!result.has_value()) - { - RCLCPP_ERROR( - this->get_logger(), "FATAL ERROR: WE SHOULD NEVER GET HERE!!"); - return; - } - rmf_chope_msgs::msg::ReservationAllocation allocation; - bool within_request = false; - for (std::size_t i = 0; i < requests_[next_item.value()].size(); - i++) - { - if (requests_[next_item.value()][i].location == result.value()) - { - allocation.satisfies_alternative = i; - within_request = true; - } - } - if (within_request) - { - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - result.value().c_str(), request->ticket.ticket_id); - allocation_pub_->publish(allocation); - } - else - { - RCLCPP_ERROR( - this->get_logger(), "FATAL ERROR: WE SHOULD NEVER GET HERE!!"); - return; - } - allocation.ticket = ticket_store_.get_existing_ticket( - request->ticket.ticket_id); - allocation.instruction_type = - rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - result.value().c_str(), request->ticket.ticket_id); - allocation_pub_->publish(allocation); + if (!result.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the chope node's state and the", ticket); + return; } + rmf_chope_msgs::msg::ReservationAllocation allocation; + allocation.satisfies_alternative = result.value(); + allocation.resource = requests_[next_item.value()][result.value()].location; + allocation.ticket = ticket_store_.get_existing_ticket( + request->ticket.ticket_id); + allocation.instruction_type = + rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + allocation.resource.c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); } @@ -548,6 +547,7 @@ class ChopeNode : public rclcpp::Node free_spot_pub_; std::unordered_map> requests_; + std::unordered_map> waitpoints_; TicketStore ticket_store_; CurrentState current_state_; ServiceQueueManager queue_manager_; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 2c870ee14..2a3c2e5a9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -17,6 +17,7 @@ #include "GoToPlace.hpp" #include "../project_itinerary.hpp" +#include "internal_ChopeNegotiator.hpp" #include "PerformAction.hpp" #include @@ -168,123 +169,6 @@ auto GoToPlace::Active::make( return active; } - active->_reservation_id = active->_context->last_reservation_request_id(); - active->_reservation_ticket = - active->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( - active->_context->worker())) - .subscribe([w = - active->weak_from_this()](const std::shared_ptr - & - msg) - { - - const auto self = w.lock(); - if (!self) - return; - - RCLCPP_ERROR( - self->_context->node()->get_logger(), - "Got ticket issueing claim"); - - if (msg->header.request_id != self->_reservation_id - || msg->header.robot_name != self->_context->name() - || msg->header.fleet_name != self->_context->group()) - { - return; - } - - self->_ticket = msg; - - - // Pick the nearest location to wait - auto current_location = self->_context->location(); - if (current_location.size() == 0) - { - return; - } - - // Order wait points by the distance from the destination. - std::vector> waitpoints_order; - for (std::size_t wp_idx = 0; - wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) - { - const auto wp = self->_context->navigation_graph().get_waypoint( - wp_idx); - - // Wait at parking spot and check its on same floor. - if (!wp.is_parking_spot() || - wp.get_map_name() != self->_context->map()) - { - continue; - } - - auto result = - self->_context->planner()->quickest_path(current_location, wp_idx); - if (!result.has_value()) - { - continue; - } - - std::stringstream json_stream; - json_stream << wp_idx << std::endl; - std::string json; - json_stream >> json; - waitpoints_order.emplace_back(result->cost(), json); - } - - std::sort(waitpoints_order.begin(), waitpoints_order.end(), - [](const std::pair& a, const std::pair& b) - { - return a.first < b.first; - }); - - // Immediately make claim cause we don't yet support flexible reservations. - rmf_chope_msgs::msg::ClaimRequest claim_request; - claim_request.ticket = *msg; - std::vector waitpoints; - for (auto&[_, waitpoint]: waitpoints_order) - { - claim_request.wait_points.push_back(waitpoint); - } - self->_context->node()->claim_location_ticket()->publish(claim_request); - RCLCPP_ERROR( - self->_context->node()->get_logger(), - "Claim issued"); - }); - - active->_reservation_allocation = - active->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( - active->_context->worker())) - .subscribe([w = - active->weak_from_this()](const std::shared_ptr - &msg) - { - const auto self = w.lock(); - if (!self) - return; - - if (!self->_ticket.has_value()) - { - return; - } - - if (msg->ticket.ticket_id != self->_ticket.value()->ticket_id) - { - return; - } - - self->_final_allocated_destination = msg; - if (msg->instruction_type - == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) - { - self->_current_reservation_state = ReservationState::RecievedResponse; - self->_retry_timer->cancel(); - self->_find_plan(); - } - }); - - active->_negotiator = Negotiator::make( active->_context, @@ -382,7 +266,29 @@ auto GoToPlace::Active::make( } }); - active->_find_plan(); + if (!active->_context->_parking_spot_manager_enabled()) + { + // If no parking spot manager is enabled then we + // just proceed to find plans as is. + active->_find_plan(); + } + else { + using namespace std::placeholders; + + // This is the parking spot manager. + active->_is_final_destination = false; + + RCLCPP_INFO(active->_context->node()->get_logger(), + "Creating Chope negotiator"); + active->_chope_client = chope::ChopeNodeNegotiator::make( + active->_context, + active->_description.one_of(), + active->_description.prefer_same_map(), + std::bind(&GoToPlace::Active::_on_chope_node_allocate_final_destination, active, _1), + std::bind(&GoToPlace::Active::_on_chope_node_allocate_waitpoint, active, _1) + ); + }; + return active; } @@ -392,6 +298,31 @@ auto GoToPlace::Active::state() const -> ConstStatePtr return _state; } +//============================================================================== +void GoToPlace::Active::_on_chope_node_allocate_final_destination( + const rmf_traffic::agv::Plan::Goal& goal) +{ + RCLCPP_INFO(_context->node()->get_logger(), + "Received final destination from chope node"); + _is_final_destination = true; + if (!_reached_waitpoint) + { + _stop_and_clear(); + } + _chosen_goal = goal; + _find_plan(); +} + +//============================================================================== +void GoToPlace::Active::_on_chope_node_allocate_waitpoint( + const rmf_traffic::agv::Plan::Goal& goal) +{ + RCLCPP_INFO(_context->node()->get_logger(), + "Received waitpoint from chope node"); + _chosen_goal = goal; + _find_plan(); +} + //============================================================================== rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const { @@ -549,119 +480,7 @@ std::optional GoToPlace::Active::_choose_goal( { RCLCPP_INFO(_context->node()->get_logger(), "Requesting Chope Node For Time To Progress"); - // No need to use reservation system if we are already there. - if (_description.one_of().size() == 1 - && _description.one_of()[0].waypoint() == current_location[0].waypoint() - && _context->_has_ticket()) - { - return _description.one_of()[0]; - } - - if (_current_reservation_state == ReservationState::Pending) - { - // Select node - rmf_chope_msgs::msg::FlexibleTimeRequest ftr; - ftr.header.robot_name = _context->name(); - ftr.header.fleet_name = _context->group(); - ftr.header.request_id = _reservation_id; - - auto lowest_cost = std::numeric_limits::infinity(); - std::optional selected_idx; - for (std::size_t i = 0; i < _description.one_of().size(); ++i) - { - const auto wp_idx = _description.one_of()[i].waypoint(); - if (only_same_map) - { - const auto& wp = graph.get_waypoint(wp_idx); - - // Check if same map. If not don't consider location. This is to ensure - // the robot does not try to board a lift. - if (wp.get_map_name() != _context->map()) - { - RCLCPP_INFO( - _context->node()->get_logger(), - "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", - wp_idx, - wp.get_map_name().c_str(), - _context->map().c_str()); - continue; - } - } - - // Find distance to said point - auto result = - _context->planner()->quickest_path(current_location, wp_idx); - if (result.has_value()) - { - RCLCPP_INFO( - _context->node()->get_logger(), - "Got distance from [%lu] as %f", - wp_idx, - result->cost()); - - if (result->cost() < lowest_cost) - { - selected_idx = i; - lowest_cost = result->cost(); - } - - std::stringstream json_stream; - json_stream << wp_idx << std::endl; - std::string json; - json_stream >> json; - rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; - alternative.resource_name = json; - alternative.cost = result->cost(); - alternative.has_end = false; - - rmf_chope_msgs::msg::StartTimeRange start; - start.earliest_start_time = _context->node()->get_clock()->now(); - start.latest_start_time = start.earliest_start_time; - start.has_earliest_start_time = true; - start.has_latest_start_time = true; - alternative.start_time = start; - - ftr.alternatives.push_back(alternative); - } - else - { - RCLCPP_ERROR( - _context->node()->get_logger(), - "No path found for robot [%s] to waypoint [%lu]", - wp_idx, - _context->requester_id().c_str()); - } - } - _context->node()->location_requester()->publish(ftr); - _current_reservation_state = ReservationState::Requested; - } - else if (_current_reservation_state == ReservationState::RecievedResponse) - { - // Go to recommended destination - if (_final_allocated_destination.value()->instruction_type == - rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) - { - RCLCPP_INFO( - _context->node()->get_logger(), - "Recieved proceed to final destination from chope node %d: ", - _final_allocated_destination.value()); - _context->_set_allocated_destination( - *_final_allocated_destination.value().get()); - return _description.one_of()[_final_allocated_destination.value()-> - satisfies_alternative]; - } - // For queueing system in future - //return rmf_traffic::agv::Plan::Goal(std::stoul(_final_allocated_destination.value()->resource), std::nullopt); - - // For now just keep retrying until your turn. - // Probably not needed since, the node does not publish an allocation till its available? - RCLCPP_ERROR( - _context->node()->get_logger(), - "Chope node said its unable to immediately service call, awaiting more." - ); - return std::nullopt; - } } else { @@ -916,10 +735,26 @@ void GoToPlace::Active::_execute_plan( .name_or_index().c_str(), _context->requester_id().c_str()); - _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), std::move(goal), - std::move(full_itinerary), - _assign_id, _state, _update, _finished, _tail_period); + + // If we use the parking spot manager, the goal may be the final destination + // or some waiting point. + if (_is_final_destination) + { + _execution = ExecutePlan::make( + _context, plan_id, std::move(plan), std::move(goal), + std::move(full_itinerary), + _assign_id, _state, _update, _finished, _tail_period); + } + else + { + _execution = ExecutePlan::make( + _context, plan_id, std::move(plan), std::move(goal), + std::move(full_itinerary), + _assign_id, _state, _update, [&](){ + _reached_waitpoint = true; + }, _tail_period); + } + if (!_execution.has_value()) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 2d9e1c65f..bdedbebc0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -24,6 +24,7 @@ #include "../services/FindPath.hpp" #include "ExecutePlan.hpp" +#include "internal_ChopeNegotiator.hpp" #include #include @@ -132,6 +133,12 @@ class GoToPlace : public rmf_task_sequence::Event void _stop_and_clear(); + void _on_chope_node_allocate_final_destination( + const rmf_traffic::agv::Plan::Goal& goal); + + void _on_chope_node_allocate_waitpoint( + const rmf_traffic::agv::Plan::Goal& goal); + Negotiator::NegotiatePtr _respond( const Negotiator::TableViewerPtr& table_view, const Negotiator::ResponderPtr& responder); @@ -154,16 +161,11 @@ class GoToPlace : public rmf_task_sequence::Event rmf_rxcpp::subscription_guard _replan_request_subscription; rmf_rxcpp::subscription_guard _graph_change_subscription; - rmf_rxcpp::subscription_guard _reservation_ticket; - rmf_rxcpp::subscription_guard _reservation_allocation; - - uint64_t _reservation_id = 0; - std::optional> _ticket{std:: - nullopt}; - std::optional> - _final_allocated_destination{std::nullopt}; + std::shared_ptr _chope_client; bool _is_interrupted = false; + bool _is_final_destination = true; + bool _reached_waitpoint = false; }; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp new file mode 100644 index 000000000..a2326298e --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -0,0 +1,319 @@ +/* + * Copyright (C) 2024 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 SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_CHOPE_NEGOTIATOR_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_CHOPE_NEGOTIATOR_HPP + +#include "../agv/RobotContext.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace chope { + +/// This class implements the protocol for negotiating a spot with the "chope" +/// node. The chope node maintains a list of spots which are free. +class ChopeNodeNegotiator: + public std::enable_shared_from_this +{ +public: + static std::shared_ptr make( + std::shared_ptr context, + const std::vector goals, + const bool same_map, + const std::function + selected_final_destination_cb, + const std::function selected_waitpoint_cb) + { + + auto negotiator = std::make_shared(); + + RCLCPP_INFO(context->node()->get_logger(), + "Creating Chope negotiator3242"); + negotiator->_context = context; + negotiator->_goals = std::move(goals); + RCLCPP_INFO(context->node()->get_logger(), + "Creating Chope negotiatoer"); + negotiator->_selected_final_destination_cb = std::move(selected_final_destination_cb); + negotiator->_selected_waitpoint_cb = std::move(selected_waitpoint_cb); + negotiator->_reservation_id = negotiator->_context->last_reservation_request_id(); + negotiator->_reservation_ticket = + negotiator->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( + negotiator->_context->worker())) + .subscribe([w = + negotiator->weak_from_this()](const std::shared_ptr + &msg) + { + + const auto self = w.lock(); + if (!self) + return; + + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Got ticket issueing claim"); + + if (msg->header.request_id != self->_reservation_id + || msg->header.robot_name != self->_context->name() + || msg->header.fleet_name != self->_context->group()) + { + return; + } + + self->_ticket = msg; + + + // Pick the nearest location to wait + auto current_location = self->_context->location(); + if (current_location.size() == 0) + { + return; + } + + // Order wait points by the distance from the destination. + std::vector> waitpoints_order; + for (std::size_t wp_idx = 0; + wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) + { + const auto wp = self->_context->navigation_graph().get_waypoint( + wp_idx); + + // Wait at parking spot and check its on same floor. + if (!wp.is_parking_spot() || + wp.get_map_name() != self->_context->map()) + { + continue; + } + + auto result = + self->_context->planner()->quickest_path(current_location, wp_idx); + if (!result.has_value()) + { + continue; + } + + std::stringstream json_stream; + json_stream << wp_idx << std::endl; + std::string json; + json_stream >> json; + waitpoints_order.emplace_back(result->cost(), json); + } + + std::sort(waitpoints_order.begin(), waitpoints_order.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); + + // Immediately make claim cause we don't yet support flexible reservations. + rmf_chope_msgs::msg::ClaimRequest claim_request; + claim_request.ticket = *msg; + std::vector waitpoints; + for (auto&[_, waitpoint]: waitpoints_order) + { + claim_request.wait_points.push_back(waitpoint); + } + self->_context->node()->claim_location_ticket()->publish(claim_request); + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Claim issued"); + }); + + + negotiator->_reservation_allocation = + negotiator->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( + negotiator->_context->worker())) + .subscribe([w = + negotiator->weak_from_this()](const std::shared_ptr + &msg) + { + const auto self = w.lock(); + if (!self) + return; + + if (!self->_ticket.has_value()) + { + return; + } + + if (msg->ticket.ticket_id != self->_ticket.value()->ticket_id) + { + return; + } + + self->_final_allocated_destination = msg; + if (msg->instruction_type + == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + { + self->_current_reservation_state = ReservationState::RecievedResponseProceedImmediate; + self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> + satisfies_alternative]); + } + + if (msg->instruction_type + == rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) + { + self->_current_reservation_state = ReservationState::RecievedResponceProceedWaitPoint; + self->_selected_waitpoint_cb(self->_waitpoints[self->_final_allocated_destination.value()-> + satisfies_alternative]); + } + }); + + RCLCPP_INFO(context->node()->get_logger(), + "Creating Chope negotiatoerwwqejnw"); + negotiator->make_request(same_map); + return negotiator; + } + +ChopeNodeNegotiator() + { + + } +private: + enum class ReservationState + { + Pending=0, + Requested=1, + RecievedResponceProceedWaitPoint=2, + RecievedResponseProceedImmediate=3 + }; + + + + void make_request(bool only_same_map) + { + //auto _context = _context.lock(); + auto current_location = _context->location(); + auto graph = _context->navigation_graph(); + if (current_location.size() == 0) + { + //unable to get location. We should return some form of error stste. + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] can't get location", + _context->requester_id().c_str()); + return; + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Selecting a new go_to_place location from [%lu] choices for robot [%s]", + _goals.size(), + _context->requester_id().c_str()); + + if (_current_reservation_state == ReservationState::Pending) + { + // Select node + rmf_chope_msgs::msg::FlexibleTimeRequest ftr; + ftr.header.robot_name = _context->name(); + ftr.header.fleet_name = _context->group(); + ftr.header.request_id = _reservation_id; + + auto lowest_cost = std::numeric_limits::infinity(); + std::optional selected_idx; + for (std::size_t i = 0; i < _goals.size(); ++i) + { + const auto wp_idx = _goals[i].waypoint(); + if (only_same_map) + { + const auto& wp = graph.get_waypoint(wp_idx); + + // Check if same map. If not don't consider location. This is to ensure + // the robot does not try to board a lift. + if (wp.get_map_name() != _context->map()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + _context->map().c_str()); + continue; + } + } + + // Find distance to said point + auto result = + _context->planner()->quickest_path(current_location, wp_idx); + if (result.has_value()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Got distance from [%lu] as %f", + wp_idx, + result->cost()); + + if (result->cost() < lowest_cost) + { + selected_idx = i; + lowest_cost = result->cost(); + } + + std::stringstream json_stream; + json_stream << wp_idx << std::endl; + std::string json; + json_stream >> json; + + rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; + alternative.resource_name = json; + alternative.cost = result->cost(); + alternative.has_end = false; + + rmf_chope_msgs::msg::StartTimeRange start; + start.earliest_start_time = _context->node()->get_clock()->now(); + start.latest_start_time = start.earliest_start_time; + start.has_earliest_start_time = true; + start.has_latest_start_time = true; + alternative.start_time = start; + + ftr.alternatives.push_back(alternative); + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + _context->requester_id().c_str(), + wp_idx); + } + } + _context->node()->location_requester()->publish(ftr); + _current_reservation_state = ReservationState::Requested; + } + } + + ReservationState _current_reservation_state = ReservationState::Pending; + std::shared_ptr _context; + std::function _selected_waitpoint_cb; + std::function _selected_final_destination_cb; + rmf_rxcpp::subscription_guard _reservation_ticket; + rmf_rxcpp::subscription_guard _reservation_allocation; + + uint64_t _reservation_id = 0; + std::optional> _ticket{std:: + nullopt}; + std::optional> + _final_allocated_destination{std::nullopt}; + + std::vector _goals; + std::vector _waitpoints; +}; +} // namespace rmf_fleet_adapter +} // namespace + +#endif \ No newline at end of file From ae62f4f6f80815f4b74d2755889d3c35b3be31bf Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 31 Jul 2024 14:47:50 +0800 Subject: [PATCH 064/119] Basic queueing is working There is still a memory leak in here somewhere as I've managed to introduce a circular reference to `GoToPlace` somehow. Also mid-route cancellation leads to segfaults. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 44 ++++++++++----- .../rmf_fleet_adapter/events/ExecutePlan.hpp | 1 + .../rmf_fleet_adapter/events/GoToPlace.cpp | 55 ++++++++++++++----- .../rmf_fleet_adapter/events/GoToPlace.hpp | 1 + .../events/internal_ChopeNegotiator.hpp | 32 ++++++----- 5 files changed, 93 insertions(+), 40 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index af31039b7..d0fabe2bf 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -177,11 +177,8 @@ class CurrentState { if (_ticket_to_location.count(ticket_id) != 0) { - /// This should never be reached. If this is reached it means that some - /// client fleetadapter node is misbehaving and requesting the same ticket - /// twice. - std::cerr << "Ticket already allocated" << std::endl; - return std::nullopt; + /// Release previous instance and reallocate ticket + release(ticket_id); } std::unordered_map positions; @@ -232,6 +229,7 @@ class CurrentState } auto location = _ticket->second; _current_location_reservations[location].ticket = std::nullopt; +_ticket_to_location.erase(_ticket); return {location}; } @@ -251,6 +249,7 @@ class ItemQueue /// Time complexity: O(1) void add(T item) { + std::cout << "Adding " << item << "to queue" << std::endl; index_to_item[curr_index] = item; item_to_index[item] = curr_index; indices.insert(curr_index); @@ -305,6 +304,7 @@ class ServiceQueueManager auto item = resource_queues[resource].front(); if (!item.has_value()) { + std::cout << "Resource " << resource << "is empty" << std::endl; return std::nullopt; } @@ -319,6 +319,8 @@ class ServiceQueueManager { for(auto resource: resources) { +std::cout << "Queue for :" << resource < locations; + std::vector location_names; for (auto location_pref: requests_[request->ticket.ticket_id]) { locations.push_back(location_pref); + location_names.push_back(location_pref.location); } // Allocate the lowest cost free spot from list of intended final locations if possible @@ -444,9 +448,14 @@ class ChopeNode : public rclcpp::Node allocation.resource.c_str(), request->ticket.ticket_id); allocation_pub_->publish(allocation); return; - } + // If we can't proceed immediately add the ticket to a queue. + RCLCPP_INFO(this->get_logger(), "Could not immediately service %lu, enqueing.", + request->ticket.ticket_id); + queue_manager_.add_to_queue(request->ticket.ticket_id, location_names); + + // Allocate a waitpoint by preference as given by Fleet Adapter std::vector wait_points; auto cost = 0.0; for (auto waitpoint_name: request->wait_points) @@ -461,7 +470,7 @@ class ChopeNode : public rclcpp::Node } waitpoints_[request->ticket.ticket_id] = wait_points; - auto waitpoint_result = current_state_.allocate_lowest_cost_free_spot(locations, + auto waitpoint_result = current_state_.allocate_lowest_cost_free_spot(wait_points, request->ticket.ticket_id); if (waitpoint_result.has_value()) { @@ -470,12 +479,15 @@ class ChopeNode : public rclcpp::Node request->ticket.ticket_id); allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY; - allocation.satisfies_alternative = result.value(); - allocation.resource = wait_points[result.value()].location; + allocation.satisfies_alternative = waitpoint_result.value(); + allocation.resource = wait_points[waitpoint_result.value()].location; + RCLCPP_INFO(this->get_logger(), "Allocating %s as waitpoint to %lu", + allocation.resource.c_str(), request->ticket.ticket_id); + allocation_pub_->publish(allocation); } else { - RCLCPP_ERROR(this->get_logger(), "Could not allocate a waiting point for robots"); + RCLCPP_ERROR(this->get_logger(), "Could not allocate a waiting point for robots from %lu waitpoints", wait_points.size()); } } @@ -498,11 +510,18 @@ class ChopeNode : public rclcpp::Node auto next_item = queue_manager_.service_next_in_queue(released_location.value()); if (!next_item.has_value()) { + RCLCPP_INFO( + this->get_logger(), "Queue is now empty %s", + released_location->c_str()); return; } // Will go to lowest auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_item.value()], next_item.value()); + RCLCPP_INFO( + this->get_logger(), "Found next item %lu on queue %s", + next_item.value(), + released_location.value().c_str()); if (!result.has_value()) { @@ -513,12 +532,11 @@ class ChopeNode : public rclcpp::Node rmf_chope_msgs::msg::ReservationAllocation allocation; allocation.satisfies_alternative = result.value(); allocation.resource = requests_[next_item.value()][result.value()].location; - allocation.ticket = ticket_store_.get_existing_ticket( - request->ticket.ticket_id); + allocation.ticket = ticket_store_.get_existing_ticket(next_item.value()); allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - allocation.resource.c_str(), request->ticket.ticket_id); + allocation.resource.c_str(), next_item.value()); allocation_pub_->publish(allocation); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp index 703349d35..70ca490ec 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp @@ -1,3 +1,4 @@ + /* * Copyright (C) 2021 Open Source Robotics Foundation * diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 2a3c2e5a9..5a8482e5e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -199,7 +199,15 @@ auto GoToPlace::Active::make( if (const auto c = self->_context->command()) c->stop(); - self->_find_plan(); + + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Goal selected %lu, %d", + self->_chosen_goal.value().waypoint(), + self->_task_finished); + + if (!self->_task_finished) + self->_find_plan(); } }); @@ -277,6 +285,7 @@ auto GoToPlace::Active::make( // This is the parking spot manager. active->_is_final_destination = false; + active->_chosen_goal = std::nullopt; RCLCPP_INFO(active->_context->node()->get_logger(), "Creating Chope negotiator"); @@ -549,7 +558,7 @@ void GoToPlace::Active::_find_plan() if (_is_interrupted) return; - if (!_chosen_goal.has_value() && _description.prefer_same_map()) + if (!_chosen_goal.has_value() && _description.prefer_same_map() ) { _chosen_goal = _choose_goal(true); } @@ -688,7 +697,9 @@ void GoToPlace::Active::_schedule_retry() const auto self = w.lock(); if (!self) return; - + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Retry timer rerunning _find_plan"); self->_retry_timer = nullptr; if (self->_execution.has_value()) return; @@ -728,22 +739,28 @@ void GoToPlace::Active::_execute_plan( const auto& graph = _context->navigation_graph(); - RCLCPP_INFO( - _context->node()->get_logger(), - "Executing go_to_place [%s] for robot [%s]", - graph.get_waypoint(plan.get_waypoints().back().graph_index().value()) - .name_or_index().c_str(), - _context->requester_id().c_str()); - // If we use the parking spot manager, the goal may be the final destination // or some waiting point. if (_is_final_destination) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Executing final go_to_place [%s] for robot [%s]", + graph.get_waypoint(plan.get_waypoints().back().graph_index().value()) + .name_or_index().c_str(), + _context->requester_id().c_str()); _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), - _assign_id, _state, _update, _finished, _tail_period); + _assign_id, _state, _update, [&](){ + RCLCPP_INFO( + _context->node()->get_logger(), + "Chope: Finished execution"); + _task_finished = true; + _stop_and_clear(); + _finished(); + }, _tail_period); } else { @@ -751,6 +768,13 @@ void GoToPlace::Active::_execute_plan( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), _assign_id, _state, _update, [&](){ + RCLCPP_INFO( + _context->node()->get_logger(), + "Chope: Reached waitpoint [%s] for robot [%s]", + graph.get_waypoint(plan.get_waypoints().back().graph_index().value()) + .name_or_index().c_str(), + _context->requester_id().c_str()); + _reached_waitpoint = true; }, _tail_period); } @@ -789,7 +813,7 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( return nullptr; } - auto approval_cb = [w = weak_from_this(), goal = *_chosen_goal]( + auto approval_cb = [w = weak_from_this()]( const rmf_traffic::PlanId plan_id, const rmf_traffic::agv::Plan& plan, rmf_traffic::schedule::Itinerary itinerary) @@ -797,8 +821,11 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( { if (auto self = w.lock()) { - self->_execute_plan(plan_id, plan, std::move(itinerary), goal); - return self->_context->itinerary().version(); + if (self->_chosen_goal.has_value()) + { + self->_execute_plan(plan_id, plan, std::move(itinerary), self->_chosen_goal.value()); + return self->_context->itinerary().version(); + } } return std::nullopt; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index bdedbebc0..d8be8ad5b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -166,6 +166,7 @@ class GoToPlace : public rmf_task_sequence::Event bool _is_interrupted = false; bool _is_final_destination = true; bool _reached_waitpoint = false; + bool _task_finished = false; }; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index a2326298e..620316fa1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -41,13 +41,8 @@ class ChopeNodeNegotiator: { auto negotiator = std::make_shared(); - - RCLCPP_INFO(context->node()->get_logger(), - "Creating Chope negotiator3242"); negotiator->_context = context; negotiator->_goals = std::move(goals); - RCLCPP_INFO(context->node()->get_logger(), - "Creating Chope negotiatoer"); negotiator->_selected_final_destination_cb = std::move(selected_final_destination_cb); negotiator->_selected_waitpoint_cb = std::move(selected_waitpoint_cb); negotiator->_reservation_id = negotiator->_context->last_reservation_request_id(); @@ -76,7 +71,6 @@ class ChopeNodeNegotiator: self->_ticket = msg; - // Pick the nearest location to wait auto current_location = self->_context->location(); if (current_location.size() == 0) @@ -85,7 +79,8 @@ class ChopeNodeNegotiator: } // Order wait points by the distance from the destination. - std::vector> waitpoints_order; + std::vector> + waitpoints_order; for (std::size_t wp_idx = 0; wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) { @@ -110,23 +105,26 @@ class ChopeNodeNegotiator: json_stream << wp_idx << std::endl; std::string json; json_stream >> json; - waitpoints_order.emplace_back(result->cost(), json); + + rmf_traffic::agv::Plan::Goal goal(wp_idx); + waitpoints_order.emplace_back(result->cost(), json, goal); } std::sort(waitpoints_order.begin(), waitpoints_order.end(), - [](const std::pair& a, const std::pair& b) + [](const std::tuple& a, + const std::tuple& b) { - return a.first < b.first; + return std::get<0>(a) < std::get<0>(b); }); // Immediately make claim cause we don't yet support flexible reservations. rmf_chope_msgs::msg::ClaimRequest claim_request; claim_request.ticket = *msg; std::vector waitpoints; - for (auto&[_, waitpoint]: waitpoints_order) + for (auto &[_, waitpoint, waitpoint_goal]: waitpoints_order) { claim_request.wait_points.push_back(waitpoint); + self->_waitpoints.push_back(waitpoint_goal); } self->_context->node()->claim_location_ticket()->publish(claim_request); RCLCPP_ERROR( @@ -157,9 +155,14 @@ class ChopeNodeNegotiator: } self->_final_allocated_destination = msg; + self->_context->_set_allocated_destination(*msg.get()); + if (msg->instruction_type == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { + RCLCPP_INFO( + self->_context->node()->get_logger(), "chope: Robot %s is going to final destination", + self->_context->name().c_str()); self->_current_reservation_state = ReservationState::RecievedResponseProceedImmediate; self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> satisfies_alternative]); @@ -171,11 +174,14 @@ class ChopeNodeNegotiator: self->_current_reservation_state = ReservationState::RecievedResponceProceedWaitPoint; self->_selected_waitpoint_cb(self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative]); + RCLCPP_INFO( + self->_context->node()->get_logger(), "chope: Robot %s is being asked to proceed to a to waitpoint", + self->_context->name().c_str()); } }); RCLCPP_INFO(context->node()->get_logger(), - "Creating Chope negotiatoerwwqejnw"); + "Sending chope request"); negotiator->make_request(same_map); return negotiator; } From e39f83ca5a2c5134189e83f9af8ff1d44ce3197e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 31 Jul 2024 15:39:21 +0800 Subject: [PATCH 065/119] Looks like the segfault was thanks to some logging It seems to be working now. Minus the memory leak. Will have to debug that. Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/events/GoToPlace.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 5a8482e5e..459f49410 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -768,12 +768,9 @@ void GoToPlace::Active::_execute_plan( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), _assign_id, _state, _update, [&](){ - RCLCPP_INFO( + RCLCPP_INFO( _context->node()->get_logger(), - "Chope: Reached waitpoint [%s] for robot [%s]", - graph.get_waypoint(plan.get_waypoints().back().graph_index().value()) - .name_or_index().c_str(), - _context->requester_id().c_str()); + "Chope: Reached waitpoint"); _reached_waitpoint = true; }, _tail_period); From 19e8618e590a48e62fea12bf984cd7283f18d34b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 1 Aug 2024 04:11:17 -0400 Subject: [PATCH 066/119] So... I know how to fix the leak but that version does not work Signed-off-by: Arjo Chakravarty --- .../events/EmergencyPullover.cpp | 239 ++++++++++++++++-- .../events/EmergencyPullover.hpp | 10 + .../services/FindEmergencyPullover.cpp | 4 +- .../services/FindEmergencyPullover.hpp | 4 - .../detail/impl_FindEmergencyPullover.hpp | 2 +- .../services/test_FindEmergencyPullover.cpp | 14 +- 6 files changed, 234 insertions(+), 39 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index b87759a7c..1bf017e49 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -22,10 +22,44 @@ #include #include #include +#include "../project_itinerary.hpp" namespace rmf_fleet_adapter { namespace events { +namespace { +//============================================================================== +std::string wp_name( + const agv::RobotContext& context, + const rmf_traffic::agv::Plan::Goal& goal) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& wp = g.get_waypoint(goal.waypoint()); + if (wp.name()) + return *wp.name(); + + return "#" + std::to_string(goal.waypoint()); +} + +//============================================================================== +std::string wp_name(const agv::RobotContext& context) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& locations = context.location(); + for (const auto& l : locations) + { + const auto& wp = g.get_waypoint(l.waypoint()); + if (wp.name()) + return *wp.name(); + } + + if (locations.empty()) + return ""; + + return "#" + std::to_string(locations.front().waypoint()); +} +} + //============================================================================== class EmergencyPulloverDescription : public rmf_task_sequence::events::Placeholder::Description @@ -155,7 +189,89 @@ auto EmergencyPullover::Active::make( return nullptr; }); - active->_find_plan(); + if (!active->_context->_parking_spot_manager_enabled()) + { + // If no parking spot manager is enabled then we + // just proceed to find plans using the old method. + active->_find_plan(); + } + else + { + // Use chope node to retrieve the best emergency location. + active->_chosen_goal = std::nullopt; + + // Find spots on same floor and order them by distance + auto current_location = active->_context->location(); + if (current_location.size() == 0) + { + // Could not localize should probably log an error and move into a + // retry timer of some form. + return active; + } + std::vector> + waitpoints_order; + for (std::size_t wp_idx = 0; + wp_idx < active->_context->navigation_graph().num_waypoints(); wp_idx++) + { + const auto wp = active->_context->navigation_graph().get_waypoint( + wp_idx); + + // Wait at parking spot and check its on same floor. + if (!wp.is_parking_spot() || + wp.get_map_name() != active->_context->map()) + { + continue; + } + + auto result = + active->_context->planner()->quickest_path(current_location, wp_idx); + if (!result.has_value()) + { + continue; + } + + rmf_traffic::agv::Plan::Goal goal(wp_idx); + waitpoints_order.emplace_back(result->cost(), goal); + } + + std::sort(waitpoints_order.begin(), waitpoints_order.end(), + [](const std::tuple& a, + const std::tuple& b) + { + return std::get<0>(a) < std::get<0>(b); + }); + + std::vector potential_waitpoints; + for (auto &[_, wp]: waitpoints_order) + { + potential_waitpoints.push_back(wp); + } + + RCLCPP_INFO(active->_context->node()->get_logger(), + "Creating Chope negotiator"); + active->_chope_client = chope::ChopeNodeNegotiator::make( + active->_context, + potential_waitpoints, + true, + [w = active->weak_from_this()](const rmf_traffic::agv::Plan::Goal& goal) + { + auto self = w.lock(); + if (!self) + return; + self->_chosen_goal = goal; + self->_find_plan(); + }, + [w = active->weak_from_this()](const rmf_traffic::agv::Plan::Goal& goal) + { + auto self = w.lock(); + if (!self) + return; + + self->_chosen_goal = goal; + self->_find_plan(); + } + ); + } return active; } @@ -242,18 +358,86 @@ void EmergencyPullover::Active::_find_plan() _state->update_status(Status::Underway); _state->update_log().info("Searching for an emergency pullover"); - _find_pullover_service = std::make_shared( - _context->emergency_planner(), _context->_get_free_spots(), _context->location(), - _context->schedule()->snapshot(), - _context->itinerary().id(), _context->profile()); + if(!_context->_parking_spot_manager_enabled()) + { + _find_pullover_service = std::make_shared( + _context->emergency_planner(), _context->location(), + _context->schedule()->snapshot(), + _context->itinerary().id(), _context->profile()); + + _pullover_subscription = + rmf_rxcpp::make_job( + _find_pullover_service) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [w = weak_from_this()]( + const services::FindEmergencyPullover::Result& result) + { + const auto self = w.lock(); + if (!self) + return; + + if (!result) + { + // The planner could not find any pullover + self->_state->update_status(Status::Error); + self->_state->update_log().error("Failed to find a pullover"); + + self->_execution = std::nullopt; + self->_schedule_retry(); + + self->_context->worker().schedule( + [update = self->_update](const auto&) { update(); }); + + return; + } + + self->_state->update_status(Status::Underway); + self->_state->update_log().info("Found an emergency pullover"); + + auto full_itinerary = result->get_itinerary(); + self->_execute_plan( + self->_context->itinerary().assign_plan_id(), + *std::move(result), + std::move(full_itinerary)); + + self->_find_pullover_service = nullptr; + self->_retry_timer = nullptr; + }); + + _find_pullover_timeout = _context->node()->try_create_wall_timer( + std::chrono::seconds(10), + [ + weak_service = _find_pullover_service->weak_from_this(), + weak_self = weak_from_this() + ]() + { + if (const auto service = weak_service.lock()) + service->interrupt(); + + if (const auto self = weak_self.lock()) + self->_find_pullover_timeout = nullptr; + }); + + _update(); + } + else { + + _find_path_service = std::make_shared( + _context->planner(), _context->location(), *_chosen_goal, + _context->schedule()->snapshot(), _context->itinerary().id(), + _context->profile(), + std::chrono::seconds(5)); + + const auto start_name = wp_name(*_context); + const auto goal_name = wp_name(*_context, *_chosen_goal); - _pullover_subscription = - rmf_rxcpp::make_job( - _find_pullover_service) + _plan_subscription = rmf_rxcpp::make_job( + _find_path_service) .observe_on(rxcpp::identity_same_worker(_context->worker())) .subscribe( - [w = weak_from_this()]( - const services::FindEmergencyPullover::Result& result) + [w = weak_from_this(), start_name, goal_name, goal = *_chosen_goal]( + const services::FindPath::Result& result) { const auto self = w.lock(); if (!self) @@ -261,36 +445,48 @@ void EmergencyPullover::Active::_find_plan() if (!result) { - // The planner could not find any pullover + // The planner could not find a way to reach the goal self->_state->update_status(Status::Error); - self->_state->update_log().error("Failed to find a pullover"); + self->_state->update_log().error( + "Failed to find a plan to move from [" + + start_name + "] to [" + goal_name + "]. Will retry soon."); + // Reset the chosen goal in case this goal has become impossible to + // reach + self->_chosen_goal = std::nullopt; self->_execution = std::nullopt; self->_schedule_retry(); - self->_context->worker().schedule( - [update = self->_update](const auto&) { update(); }); + self->_context->worker() + .schedule([update = self->_update](const auto&) { update(); }); return; } self->_state->update_status(Status::Underway); - self->_state->update_log().info("Found an emergency pullover"); + self->_state->update_log().info( + "Found a plan to move from [" + + start_name + "] to [" + goal_name + "]"); + + + std::vector no_future_projections; + auto full_itinerary = project_itinerary( + *result, no_future_projections, + *self->_context->planner()); - auto full_itinerary = result->get_itinerary(); self->_execute_plan( self->_context->itinerary().assign_plan_id(), *std::move(result), std::move(full_itinerary)); - self->_find_pullover_service = nullptr; + self->_find_path_service = nullptr; self->_retry_timer = nullptr; }); - _find_pullover_timeout = _context->node()->try_create_wall_timer( + _find_path_timeout = _context->node()->try_create_wall_timer( std::chrono::seconds(10), [ - weak_service = _find_pullover_service->weak_from_this(), + weak_service = _find_path_service->weak_from_this(), weak_self = weak_from_this() ]() { @@ -298,10 +494,11 @@ void EmergencyPullover::Active::_find_plan() service->interrupt(); if (const auto self = weak_self.lock()) - self->_find_pullover_timeout = nullptr; + self->_find_path_timeout = nullptr; }); - _update(); + _update(); + } } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp index 6a4b4ca9e..1e3ced509 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp @@ -22,6 +22,9 @@ #include "../Negotiator.hpp" #include "../services/FindEmergencyPullover.hpp" +#include "../services/FindPath.hpp" +#include "internal_ChopeNegotiator.hpp" + #include "ExecutePlan.hpp" @@ -121,6 +124,13 @@ class EmergencyPullover : public rmf_task_sequence::Event rclcpp::TimerBase::SharedPtr _find_pullover_timeout; rclcpp::TimerBase::SharedPtr _retry_timer; + std::shared_ptr _find_path_service; + rmf_rxcpp::subscription_guard _plan_subscription; + rclcpp::TimerBase::SharedPtr _find_path_timeout; + std::optional _chosen_goal; + + std::shared_ptr _chope_client; + bool _is_interrupted = false; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp index a0420e26c..7d2a28b8e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.cpp @@ -23,7 +23,6 @@ namespace services { //============================================================================== FindEmergencyPullover::FindEmergencyPullover( std::shared_ptr planner, - std::unordered_set free_spots, rmf_traffic::agv::Plan::StartSet starts, std::shared_ptr schedule, rmf_traffic::schedule::ParticipantId participant_id, @@ -32,8 +31,7 @@ FindEmergencyPullover::FindEmergencyPullover( _starts(std::move(starts)), _schedule(std::move(schedule)), _participant_id(participant_id), - _profile(std::move(profile)), - _parking_spots(std::move(free_spots)) + _profile(std::move(profile)) { // Do nothing } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp index 7d12b7b90..35c597ba0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindEmergencyPullover.hpp @@ -20,8 +20,6 @@ #include "../jobs/SearchForPath.hpp" #include "ProgressEvaluator.hpp" -#include -#include namespace rmf_fleet_adapter { namespace services { @@ -34,7 +32,6 @@ class FindEmergencyPullover FindEmergencyPullover( std::shared_ptr planner, - std::unordered_set free_spots, rmf_traffic::agv::Plan::StartSet starts, std::shared_ptr schedule, rmf_traffic::schedule::ParticipantId participant_id, @@ -56,7 +53,6 @@ class FindEmergencyPullover std::shared_ptr _schedule; rmf_traffic::schedule::ParticipantId _participant_id; std::shared_ptr _profile; - std::unordered_set _parking_spots; std::vector> _search_jobs; rmf_rxcpp::subscription_guard _search_sub; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp index bf9c7a56b..89e4e5a4d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/detail/impl_FindEmergencyPullover.hpp @@ -30,7 +30,7 @@ void FindEmergencyPullover::operator()(const Subscriber& s) const auto& graph = _planner->get_configuration().graph(); const std::size_t N = graph.num_waypoints(); _search_jobs.reserve(N); - for (auto i: _parking_spots) + for (std::size_t i = 0; i < N; ++i) { const auto& wp = graph.get_waypoint(i); if (wp.is_parking_spot()) diff --git a/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp b/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp index cb272c325..808d7d8b2 100644 --- a/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp +++ b/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp @@ -118,12 +118,6 @@ SCENARIO("Emergency Pullover") rmf_traffic::agv::Planner::Options{nullptr} ); - std::unordered_set all_waypoints; - for (std::size_t i = 0; i < graph.num_waypoints(); i++) - { - all_waypoints.insert(i); - } - const auto now = std::chrono::steady_clock::now(); using namespace std::chrono_literals; @@ -134,7 +128,7 @@ SCENARIO("Emergency Pullover") auto pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, all_waypoints, + planner, rmf_traffic::agv::Plan::StartSet({start_0}), database->snapshot(), p0.id(), std::make_shared(p0.description().profile())); @@ -172,7 +166,7 @@ SCENARIO("Emergency Pullover") // want to park at waypoint 7 in the absence of any other conflicts. pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, all_waypoints, + planner, rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); @@ -214,7 +208,7 @@ SCENARIO("Emergency Pullover") pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, all_waypoints, + planner, rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); @@ -271,7 +265,7 @@ SCENARIO("Emergency Pullover") auto pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, all_waypoints, + planner, rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); From dfaa0ea6ef0f925448c995ed109c9329de69d226 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 1 Aug 2024 07:21:56 -0400 Subject: [PATCH 067/119] Fixed memory leak Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 29 +++++++++++-------- .../rmf_fleet_adapter/events/GoToPlace.hpp | 1 - 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 459f49410..28fc226a1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -202,12 +202,10 @@ auto GoToPlace::Active::make( RCLCPP_INFO( self->_context->node()->get_logger(), - "Goal selected %lu, %d", - self->_chosen_goal.value().waypoint(), - self->_task_finished); + "Goal selected %lu", + self->_chosen_goal.value().waypoint()); - if (!self->_task_finished) - self->_find_plan(); + self->_find_plan(); } }); @@ -281,8 +279,6 @@ auto GoToPlace::Active::make( active->_find_plan(); } else { - using namespace std::placeholders; - // This is the parking spot manager. active->_is_final_destination = false; active->_chosen_goal = std::nullopt; @@ -293,8 +289,18 @@ auto GoToPlace::Active::make( active->_context, active->_description.one_of(), active->_description.prefer_same_map(), - std::bind(&GoToPlace::Active::_on_chope_node_allocate_final_destination, active, _1), - std::bind(&GoToPlace::Active::_on_chope_node_allocate_waitpoint, active, _1) + [w = active->weak_from_this()]( const rmf_traffic::agv::Plan::Goal& goal) { + if (auto self = w.lock()) + { + self->_on_chope_node_allocate_final_destination(goal); + } + }, + [w = active->weak_from_this()]( const rmf_traffic::agv::Plan::Goal& goal) { + if (auto self = w.lock()) + { + self->_on_chope_node_allocate_waitpoint(goal); + } + } ); }; @@ -732,8 +738,8 @@ void GoToPlace::Active::_execute_plan( const auto& graph = _context->navigation_graph(); _context->retain_mutex_groups( {graph.get_waypoint(goal.waypoint()).in_mutex_group()}); - - _finished(); + if (_is_final_destination) + _finished(); return; } @@ -757,7 +763,6 @@ void GoToPlace::Active::_execute_plan( RCLCPP_INFO( _context->node()->get_logger(), "Chope: Finished execution"); - _task_finished = true; _stop_and_clear(); _finished(); }, _tail_period); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index d8be8ad5b..bdedbebc0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -166,7 +166,6 @@ class GoToPlace : public rmf_task_sequence::Event bool _is_interrupted = false; bool _is_final_destination = true; bool _reached_waitpoint = false; - bool _task_finished = false; }; }; From af2185d90d5eaea2a56291aef9dddee291d2a6d5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 1 Aug 2024 22:46:01 -0400 Subject: [PATCH 068/119] Finally works fully Fixed an edge case where we ask the robot to go back to the same place again. Signed-off-by: Arjo Chakravarty --- .../events/internal_ChopeNegotiator.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 620316fa1..233db6a2a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -101,6 +101,8 @@ class ChopeNodeNegotiator: continue; } + // TODO(arjo): We really should be using graph id and name here. + // For multiple graphs we need some form of mapping. std::stringstream json_stream; json_stream << wp_idx << std::endl; std::string json; @@ -222,6 +224,20 @@ ChopeNodeNegotiator() _goals.size(), _context->requester_id().c_str()); + // If we are at the goal location already do nothing. + for (auto goal: _goals) + { + if (goal.waypoint() == current_location[0].waypoint()) + { + RCLCPP_INFO( + _ context->node()->get_logger(), + "Found we were already at %lu. No need to interact with the Chope node.", + current_location[0].waypoint()); + _selected_final_destination_cb(goal); + return; + } + } + if (_current_reservation_state == ReservationState::Pending) { // Select node From 0fefbb32e24cf354fa60dc05c44db828a3af094a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 2 Aug 2024 11:45:10 +0800 Subject: [PATCH 069/119] Add docs Signed-off-by: Arjo Chakravarty --- rmf_chope_node/README.md | 55 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 rmf_chope_node/README.md diff --git a/rmf_chope_node/README.md b/rmf_chope_node/README.md new file mode 100644 index 000000000..459d43bbd --- /dev/null +++ b/rmf_chope_node/README.md @@ -0,0 +1,55 @@ +# rmf\_chope\_node package + +"Chope" is the very singaporean act of throwing an item on a table and marking it at yours before going to get your food. The goal of this package is to provide the same functionality in RMF. +Before a robot goes to its next destination the fleet adapter asks the chope-node if its next destination is available. If the destination is available the robot will immediately proceed to the next destination, otherwise it will allocate a free parking spot for the robot to wait at till the next destination becomes available in a FIFO manner. If you need more advanced methods than FIFO, feel free to hack this package. To enable the use of this package you need to add the following to your fleet configuration yaml at the fleet level and run the chope node: +```yaml + use_parking_reservations: True +``` +We recommend disabling `responsive_wait` when you do so. An example is available in `rmf_demos`. + +Some immediate limitations of the chope node is that it is limited to scenarios with a single navigation graph. Overcoming this limitation should not be too much work now that the basic infrastructure is in place. + +## Expected Behaviour + +### Basic queueing +You should never need to interact with this package directly. Rather, you can simply dispatch a task using the traditional methods. If you have `rmf_demos` installed you can run the +office world. We can start by commanding the `tinyRobot2` to go to the pantry. +```bash +ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot2 --use-sim-time +``` +The robot should proceed as expected. We can then ask `tinyRobot1` to also go to the pantry. Nothing should happen as the pantry is already occupied and `tinyRobot1` is at its parking spot. +```bash +ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot1 --use-sim-time +``` +We can ask `tinyRobot2` to move to the lounge after this. +```bash +ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot2 --use-sim-time +``` +This should immediately trigger 2 things: +1. `tinyRobot2` will move to the lounge +2. `tinyRobot1` will proceed to move to the pantry taking tinyRobot2's place. + +### Moving to a waitpoint +If we continue from the previous example. Lets ask `tinyRobot1` and `tinyRobot2` to swap places starting at the pantry and lounge respectively. +```bash +ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot1 --use-sim-time +``` +You should see tinyRobot1 move to`tinyRobot2_charger` as `lounge` is not a designated wait point. You can then command `tinyRobot2` to the lounge. +```bash +ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot2 --use-sim-time +``` +`tinyRobot1` and `tinyRobot2` should now proceed to swap places. + +If you have the same number or more waitpoints than robots you should never have a deadlock. + +## Protocol Used Behind the Scenes + +The chope node has no idea about the state of the graph it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace`. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. If we are at the target location, no comms is needed. Otherwise the fleetadapter submits a `ReservationRequest` message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The chope node will issue a ticket for said request.When ready to proceed, send a claim message witht ht e ticket and with the list of potential waiting points ordered by distance. The chope node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. + +## Known Issues +1. We can only use one nav graoh with the chope node (it should be easy to resolve this with a mapping between nav graphs but for now its not part of the initial release). +2. At start up if there is no idle task, the chope node will not know where the robots are. It is adviced to send 1 `GoToPlace` task for every robot that is added to the world. + +## Quality Declaration + +This package claims to be in the **Quality Level 4** category. See the [Quality Declaration](QUALITY_DECLARATION.md) for more details. From 3b049bc1078a6677df3072bf4d4a0251bc35d6a3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 2 Aug 2024 00:52:19 -0400 Subject: [PATCH 070/119] Whoops fix stray keystroke Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp | 5 +---- .../rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 28fc226a1..69da6444b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -320,10 +320,7 @@ void GoToPlace::Active::_on_chope_node_allocate_final_destination( RCLCPP_INFO(_context->node()->get_logger(), "Received final destination from chope node"); _is_final_destination = true; - if (!_reached_waitpoint) - { - _stop_and_clear(); - } + _chosen_goal = goal; _find_plan(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 233db6a2a..701e1ece8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -230,7 +230,7 @@ ChopeNodeNegotiator() if (goal.waypoint() == current_location[0].waypoint()) { RCLCPP_INFO( - _ context->node()->get_logger(), + context->node()->get_logger(), "Found we were already at %lu. No need to interact with the Chope node.", current_location[0].waypoint()); _selected_final_destination_cb(goal); From f90946db946e660dd65e097a62b928d4f52d1b2d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 2 Aug 2024 02:20:24 -0400 Subject: [PATCH 071/119] Release in the correct place Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 22 ------------------- .../events/internal_ChopeNegotiator.hpp | 21 ++++++++++++++++-- 2 files changed, 19 insertions(+), 24 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 330c8d67a..3a1c96b31 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -1313,28 +1313,6 @@ std::optional ExecutePlan::make( rmf_task_sequence::events::Bundle::Type::Sequence, standbys, state, std::move(update))->begin([]() {}, std::move(finished)); - if (waypoint_copy.size() > 0 && context->location().size()) - { - if (waypoint_copy.back().graph_index() != context->location()[0].waypoint()) - { - auto allocation = context->_release_resource(); - if (allocation.has_value()) - { - rmf_chope_msgs::msg::ReleaseRequest msg; - std::stringstream str; - str << context->location()[0].waypoint(); - str >> msg.location; - msg.ticket = allocation->ticket; - context->node()->release_location()->publish(msg); - RCLCPP_ERROR( - context->node()->get_logger(), - "Releasing waypoint", - waypoint_copy.size() - ); - } - - } - } return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 701e1ece8..0c632b144 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -159,6 +159,23 @@ class ChopeNodeNegotiator: self->_final_allocated_destination = msg; self->_context->_set_allocated_destination(*msg.get()); + if (self->_current_reservation_state == ReservationState::Requested) + { + while (auto allocation = self->_context->_release_resource()) + { + rmf_chope_msgs::msg::ReleaseRequest msg; + std::stringstream str; + str << self->_context->location()[0].waypoint(); + str >> msg.location; + msg.ticket = allocation->ticket; + self->_context->node()->release_location()->publish(msg); + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Releasing waypoint" + ); + } + } + if (msg->instruction_type == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { @@ -225,7 +242,7 @@ ChopeNodeNegotiator() _context->requester_id().c_str()); // If we are at the goal location already do nothing. - for (auto goal: _goals) + /*for (auto goal: _goals) { if (goal.waypoint() == current_location[0].waypoint()) { @@ -236,7 +253,7 @@ ChopeNodeNegotiator() _selected_final_destination_cb(goal); return; } - } + }*/ if (_current_reservation_state == ReservationState::Pending) { From 2c18dcbd26590bb77927626e9a0688ebcd640dda Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 8 Aug 2024 09:00:57 +0800 Subject: [PATCH 072/119] Code clean ups Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 4 ---- .../events/internal_ChopeNegotiator.hpp | 15 --------------- 2 files changed, 19 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index d0fabe2bf..13ded7d90 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -249,7 +249,6 @@ class ItemQueue /// Time complexity: O(1) void add(T item) { - std::cout << "Adding " << item << "to queue" << std::endl; index_to_item[curr_index] = item; item_to_index[item] = curr_index; indices.insert(curr_index); @@ -304,7 +303,6 @@ class ServiceQueueManager auto item = resource_queues[resource].front(); if (!item.has_value()) { - std::cout << "Resource " << resource << "is empty" << std::endl; return std::nullopt; } @@ -319,8 +317,6 @@ class ServiceQueueManager { for(auto resource: resources) { -std::cout << "Queue for :" << resource <location(); auto graph = _context->navigation_graph(); if (current_location.size() == 0) @@ -241,20 +240,6 @@ ChopeNodeNegotiator() _goals.size(), _context->requester_id().c_str()); - // If we are at the goal location already do nothing. - /*for (auto goal: _goals) - { - if (goal.waypoint() == current_location[0].waypoint()) - { - RCLCPP_INFO( - context->node()->get_logger(), - "Found we were already at %lu. No need to interact with the Chope node.", - current_location[0].waypoint()); - _selected_final_destination_cb(goal); - return; - } - }*/ - if (_current_reservation_state == ReservationState::Pending) { // Select node From 3f4bd8aa8e15e54d9f3ad7eeed526c5e28351791 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 8 Aug 2024 11:42:27 +0800 Subject: [PATCH 073/119] Fix typos Signed-off-by: Arjo Chakravarty --- rmf_chope_node/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rmf_chope_node/README.md b/rmf_chope_node/README.md index 459d43bbd..1f47f2962 100644 --- a/rmf_chope_node/README.md +++ b/rmf_chope_node/README.md @@ -15,15 +15,15 @@ Some immediate limitations of the chope node is that it is limited to scenarios You should never need to interact with this package directly. Rather, you can simply dispatch a task using the traditional methods. If you have `rmf_demos` installed you can run the office world. We can start by commanding the `tinyRobot2` to go to the pantry. ```bash -ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot2 --use-sim-time +ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot -R tinyRobot2 --use_sim_time ``` The robot should proceed as expected. We can then ask `tinyRobot1` to also go to the pantry. Nothing should happen as the pantry is already occupied and `tinyRobot1` is at its parking spot. ```bash -ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot1 --use-sim-time +ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot -R tinyRobot1 --use_sim_time ``` We can ask `tinyRobot2` to move to the lounge after this. ```bash -ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot2 --use-sim-time +ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot -R tinyRobot2 --use_sim_time ``` This should immediately trigger 2 things: 1. `tinyRobot2` will move to the lounge @@ -32,11 +32,11 @@ This should immediately trigger 2 things: ### Moving to a waitpoint If we continue from the previous example. Lets ask `tinyRobot1` and `tinyRobot2` to swap places starting at the pantry and lounge respectively. ```bash -ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot1 --use-sim-time +ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot -R tinyRobot1 --use_sim_time ``` You should see tinyRobot1 move to`tinyRobot2_charger` as `lounge` is not a designated wait point. You can then command `tinyRobot2` to the lounge. ```bash -ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot2 --use-sim-time +ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot -R tinyRobot2 --use_sim_time ``` `tinyRobot1` and `tinyRobot2` should now proceed to swap places. From 020d7b1938c8c2ab7f648ec025331e918390f4c4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 8 Aug 2024 13:15:25 +0800 Subject: [PATCH 074/119] Switched to names to support multiple graphs Signed-off-by: Arjo Chakravarty --- rmf_chope_node/README.md | 5 ++- rmf_chope_node/src/main.cpp | 6 +--- .../events/internal_ChopeNegotiator.hpp | 35 +++++++++++-------- 3 files changed, 24 insertions(+), 22 deletions(-) diff --git a/rmf_chope_node/README.md b/rmf_chope_node/README.md index 1f47f2962..0102abe07 100644 --- a/rmf_chope_node/README.md +++ b/rmf_chope_node/README.md @@ -44,11 +44,10 @@ If you have the same number or more waitpoints than robots you should never have ## Protocol Used Behind the Scenes -The chope node has no idea about the state of the graph it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace`. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. If we are at the target location, no comms is needed. Otherwise the fleetadapter submits a `ReservationRequest` message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The chope node will issue a ticket for said request.When ready to proceed, send a claim message witht ht e ticket and with the list of potential waiting points ordered by distance. The chope node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. +The chope node has no idea about the state of the graph it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace`. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleetadapter submits a `ReservationRequest` message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The chope node will issue a ticket for said request.When ready to proceed, send a claim message with the ticket and with the list of potential waiting points ordered by distance. The chope node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. ## Known Issues -1. We can only use one nav graoh with the chope node (it should be easy to resolve this with a mapping between nav graphs but for now its not part of the initial release). -2. At start up if there is no idle task, the chope node will not know where the robots are. It is adviced to send 1 `GoToPlace` task for every robot that is added to the world. +1. At start up if there is no idle task, the chope node will not know where the robots are. It is advised to send 1 `GoToPlace` task for every robot that is added to the world. ## Quality Declaration diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 13ded7d90..24f6122c8 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -383,11 +383,7 @@ class ChopeNode : public rclcpp::Node //TODO(arjoc) make this configure-able if (param.name == "is_parking_spot" && param.value_bool) { - std::stringstream name; - name << i; - std::string topic; - name >> topic; - current_state_.add_location(topic); + current_state_.add_location( graph_msg->vertices[i].name); } } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 353f160ed..c90068f16 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -87,6 +87,15 @@ class ChopeNodeNegotiator: const auto wp = self->_context->navigation_graph().get_waypoint( wp_idx); + auto name = wp.name(); + if (name == nullptr) + { + RCLCPP_ERROR(self->_context->node()->get_logger(), + "Got a parking spot without a name." + "This parking spot will not be used by the reservation system."); + continue; + } + // Wait at parking spot and check its on same floor. if (!wp.is_parking_spot() || wp.get_map_name() != self->_context->map()) @@ -101,15 +110,8 @@ class ChopeNodeNegotiator: continue; } - // TODO(arjo): We really should be using graph id and name here. - // For multiple graphs we need some form of mapping. - std::stringstream json_stream; - json_stream << wp_idx << std::endl; - std::string json; - json_stream >> json; - rmf_traffic::agv::Plan::Goal goal(wp_idx); - waitpoints_order.emplace_back(result->cost(), json, goal); + waitpoints_order.emplace_back(result->cost(), *name, goal); } std::sort(waitpoints_order.begin(), waitpoints_order.end(), @@ -253,9 +255,10 @@ ChopeNodeNegotiator() for (std::size_t i = 0; i < _goals.size(); ++i) { const auto wp_idx = _goals[i].waypoint(); + const auto& wp = graph.get_waypoint(wp_idx); + if (only_same_map) { - const auto& wp = graph.get_waypoint(wp_idx); // Check if same map. If not don't consider location. This is to ensure // the robot does not try to board a lift. @@ -288,13 +291,17 @@ ChopeNodeNegotiator() lowest_cost = result->cost(); } - std::stringstream json_stream; - json_stream << wp_idx << std::endl; - std::string json; - json_stream >> json; + auto name = wp.name(); + if (name == nullptr) + { + RCLCPP_ERROR(_context->node()->get_logger(), + "Got a parking spot without a name." + "This parking spot will not be used by the reservation system."); + continue; + } rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; - alternative.resource_name = json; + alternative.resource_name = *name; alternative.cost = result->cost(); alternative.has_end = false; From b264cfb9373c263964526efa863171ecb591891d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 14 Aug 2024 15:56:49 +0800 Subject: [PATCH 075/119] Refactoring and Stylistic changes Signed-off-by: Arjo Chakravarty --- .gitignore | 1 - rmf_chope_node/README.md | 2 +- rmf_fleet_adapter/schemas/place.json | 3 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 16 +---- .../rmf_fleet_adapter/agv/RobotContext.hpp | 19 +----- .../events/internal_ChopeNegotiator.hpp | 58 +++++++++++-------- 6 files changed, 39 insertions(+), 60 deletions(-) diff --git a/.gitignore b/.gitignore index a3a98fa38..652199902 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,3 @@ tags *.swo *.vscode compile_commands.json -rmf_reservation_ros2/target diff --git a/rmf_chope_node/README.md b/rmf_chope_node/README.md index 0102abe07..a5db2e075 100644 --- a/rmf_chope_node/README.md +++ b/rmf_chope_node/README.md @@ -51,4 +51,4 @@ The chope node has no idea about the state of the graph it simply maintains a li ## Quality Declaration -This package claims to be in the **Quality Level 4** category. See the [Quality Declaration](QUALITY_DECLARATION.md) for more details. +This package claims to be in the **Quality Level 4** category. See the [Quality Declaration](QUALITY_DECLARATION.md) for more details._ \ No newline at end of file diff --git a/rmf_fleet_adapter/schemas/place.json b/rmf_fleet_adapter/schemas/place.json index 84738967a..684a35fe6 100644 --- a/rmf_fleet_adapter/schemas/place.json +++ b/rmf_fleet_adapter/schemas/place.json @@ -11,8 +11,7 @@ "type": "object", "properties": { "waypoint": { "$ref": "#/$defs/waypoint" }, - "orientation": { "type": "number" }, - "hold_for": {"type": "number"} + "orientation": { "type": "number" } }, "required": ["waypoint"] } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 7aed9aa07..c8e301bae 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1715,13 +1715,13 @@ void RobotContext::_set_allocated_destination( //============================================================================== void RobotContext::_set_parking_spot_manager(const bool enabled) { - this->use_parking_spot_reservations = enabled; + _use_parking_spot_reservations = enabled; } //============================================================================== bool RobotContext::_parking_spot_manager_enabled() { - return this->use_parking_spot_reservations; + return _use_parking_spot_reservations; } //============================================================================== @@ -1737,18 +1737,6 @@ bool RobotContext::_has_ticket() const return _reservation_mgr.has_ticket(); } -//============================================================================== -std::unordered_set RobotContext::_get_free_spots() const -{ - // TODO(arjo129) Poor efficiency of messages. Change messages to use - // graph/vertex pair. - std::unordered_set set; - for (auto spot: _free_spots.spots) - { - set.insert(std::stoul(spot)); - } - return set; -} //============================================================================== void RobotContext::_handle_mutex_group_manual_release( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 580808ccf..555a79a99 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -778,8 +778,6 @@ class RobotContext /// Release last resource that was acquired. std::optional _release_resource(); - std::unordered_set _get_free_spots() const; - /// Has ticket now bool _has_ticket() const; @@ -848,17 +846,6 @@ class RobotContext self->_handle_mutex_group_manual_release(*msg); }); - context->_free_space_sub = context->_node->freespots_obs() - .observe_on(rxcpp::identity_same_worker(context->_worker)) - .subscribe([w = context->weak_from_this()](const auto& msg) - { - const auto self = w.lock(); - if (!self) - return; - - self->_free_spots = *msg; - }); - return context; } @@ -970,13 +957,11 @@ class RobotContext std::unordered_map _requesting_mutex_groups; std::unordered_map _locked_mutex_groups; - rmf_chope_msgs::msg::FreeParkingSpots _free_spots; - rxcpp::subjects::subject _mutex_group_lock_subject; rxcpp::observable _mutex_group_lock_obs; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; - rmf_rxcpp::subscription_guard _free_space_sub; + rclcpp::Subscription::SharedPtr _mutex_group_manual_release_sub; std::chrono::steady_clock::time_point _last_active_task_time; @@ -987,7 +972,7 @@ class RobotContext std::unique_ptr _final_lift_destination_mutex = std::make_unique(); - bool use_parking_spot_reservations; + bool _use_parking_spot_reservations; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index c90068f16..bf8cce8f7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -31,7 +31,7 @@ class ChopeNodeNegotiator: public std::enable_shared_from_this { public: - static std::shared_ptr make( + ChopeNodeNegotiator( std::shared_ptr context, const std::vector goals, const bool same_map, @@ -39,18 +39,16 @@ class ChopeNodeNegotiator: selected_final_destination_cb, const std::function selected_waitpoint_cb) { - - auto negotiator = std::make_shared(); - negotiator->_context = context; - negotiator->_goals = std::move(goals); - negotiator->_selected_final_destination_cb = std::move(selected_final_destination_cb); - negotiator->_selected_waitpoint_cb = std::move(selected_waitpoint_cb); - negotiator->_reservation_id = negotiator->_context->last_reservation_request_id(); - negotiator->_reservation_ticket = - negotiator->_context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( - negotiator->_context->worker())) + _context = context; + _goals = std::move(goals); + _selected_final_destination_cb = std::move(selected_final_destination_cb); + _selected_waitpoint_cb = std::move(selected_waitpoint_cb); + _reservation_id = _context->last_reservation_request_id(); + _reservation_ticket = + _context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( + _context->worker())) .subscribe([w = - negotiator->weak_from_this()](const std::shared_ptr + weak_from_this()](const std::shared_ptr &msg) { @@ -137,11 +135,11 @@ class ChopeNodeNegotiator: }); - negotiator->_reservation_allocation = - negotiator->_context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( - negotiator->_context->worker())) + _reservation_allocation = + _context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( + _context->worker())) .subscribe([w = - negotiator->weak_from_this()](const std::shared_ptr + weak_from_this()](const std::shared_ptr &msg) { const auto self = w.lock(); @@ -184,7 +182,7 @@ class ChopeNodeNegotiator: RCLCPP_INFO( self->_context->node()->get_logger(), "chope: Robot %s is going to final destination", self->_context->name().c_str()); - self->_current_reservation_state = ReservationState::RecievedResponseProceedImmediate; + self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> satisfies_alternative]); } @@ -192,7 +190,7 @@ class ChopeNodeNegotiator: if (msg->instruction_type == rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) { - self->_current_reservation_state = ReservationState::RecievedResponceProceedWaitPoint; + self->_current_reservation_state = ReservationState::ReceivedResponseProceedWaitPoint; self->_selected_waitpoint_cb(self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative]); RCLCPP_INFO( @@ -203,21 +201,31 @@ class ChopeNodeNegotiator: RCLCPP_INFO(context->node()->get_logger(), "Sending chope request"); - negotiator->make_request(same_map); - return negotiator; + make_request(same_map); } - -ChopeNodeNegotiator() + static std::shared_ptr make( + std::shared_ptr context, + const std::vector goals, + const bool same_map, + const std::function + selected_final_destination_cb, + const std::function selected_waitpoint_cb) { + auto negotiator = std::make_shared(context, + goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); + return negotiator; } + + private: + enum class ReservationState { Pending=0, Requested=1, - RecievedResponceProceedWaitPoint=2, - RecievedResponseProceedImmediate=3 + ReceivedResponseProceedWaitPoint=2, + ReceivedResponseProceedImmediate=3 }; @@ -225,7 +233,7 @@ ChopeNodeNegotiator() void make_request(bool only_same_map) { auto current_location = _context->location(); - auto graph = _context->navigation_graph(); + const auto& graph = _context->navigation_graph(); if (current_location.size() == 0) { //unable to get location. We should return some form of error stste. From 49a82a60c891af055da431488fc4837e033bc90c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 14 Aug 2024 17:07:04 +0800 Subject: [PATCH 076/119] Removing unused subscriber Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp | 2 -- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 3 --- 2 files changed, 5 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 840ab8a18..36ea51653 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -82,8 +82,6 @@ const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; const std::string ReservationAllocationTopicName = "/rmf/reservations/allocation"; const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; -const std::string ReservationFreeSpotsTopicName = - "/rmf/reservations/free_parking_spot"; const uint64_t Unclaimed = (uint64_t)(-1); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 36a6ef347..d4d149730 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -139,9 +139,6 @@ std::shared_ptr Node::make( node->create_publisher( ReservationReleaseTopicName, transient_local_qos); - node->_reservation_free_spot_obs = - node->create_observable( - ReservationFreeSpotsTopicName, transient_local_qos); return node; } From bdb8cc4615294f9053277553be6fd160a4aed58d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Aug 2024 15:28:28 +0800 Subject: [PATCH 077/119] More refactoring Signed-off-by: Arjo Chakravarty --- rmf_chope_node/CMakeLists.txt | 16 -- rmf_chope_node/src/main.cpp | 19 +- .../agv/ReservationManager.hpp | 2 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 85 +++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 8 + .../events/EmergencyPullover.cpp | 172 +++++++----------- .../rmf_fleet_adapter/events/GoToPlace.cpp | 9 +- .../rmf_fleet_adapter/events/GoToPlace.hpp | 11 +- .../events/internal_ChopeNegotiator.hpp | 81 ++------- .../services/test_FindEmergencyPullover.cpp | 13 +- 10 files changed, 187 insertions(+), 229 deletions(-) diff --git a/rmf_chope_node/CMakeLists.txt b/rmf_chope_node/CMakeLists.txt index 526b964fa..2cfac3fa2 100644 --- a/rmf_chope_node/CMakeLists.txt +++ b/rmf_chope_node/CMakeLists.txt @@ -10,9 +10,6 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmf_chope_msgs REQUIRED) find_package(rmf_building_map_msgs REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) add_executable(queue_manager src/main.cpp) ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs rmf_building_map_msgs) @@ -21,17 +18,4 @@ install(TARGETS queue_manager DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 24f6122c8..e0b764470 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -91,8 +91,6 @@ class TicketStore request_header.robot_name, request_header.fleet_name }; - _robots_to_tickets[robot_identifier].push_back(_last_issued_ticket_id); - _header_to_ticket.emplace(request_header, _last_issued_ticket_id); _ticket_to_header.emplace(_last_issued_ticket_id, request_header); _last_issued_ticket_id++; return ticket; @@ -106,10 +104,6 @@ class TicketStore return ticket; } - std::unordered_map> _robots_to_tickets; - std::unordered_map _header_to_ticket; std::unordered_map _ticket_to_header; std::size_t _last_issued_ticket_id = 1; @@ -145,7 +139,6 @@ class CurrentState public: std::vector free_locations() { - std::lock_guard lock(_mtx); std::vector locations; for (auto&[loc, state] : _current_location_reservations) { @@ -159,7 +152,6 @@ class CurrentState void add_location(std::string location) { - std::lock_guard lock(_mtx); if (_current_location_reservations.count(location) == 0) { _current_location_reservations.emplace(location, @@ -170,7 +162,7 @@ class CurrentState /// Tries to greedily allocate the lowest cost free spot given a list of potential /// parking spots. /// \param[in] incoming_requests - Parking spot and cost of said parking spot - /// \param[in] ticket_id - Ticket which is being + /// \param[in] ticket_id - Ticket which is being serviced std::optional allocate_lowest_cost_free_spot( const std::vector& incoming_requests, const std::size_t ticket_id) @@ -187,9 +179,8 @@ class CurrentState positions[incoming_requests[i].location] = i; } - auto requests = incoming_requests; + auto requests = std::move(incoming_requests); std::sort(requests.begin(), requests.end()); - std::lock_guard lock(_mtx); for (std::size_t i = 0; i < requests.size(); i++) { auto parking = _current_location_reservations.find(requests[i].location); @@ -221,7 +212,6 @@ class CurrentState std::optional release(const std::size_t ticket_id) { - std::lock_guard lock(_mtx); auto _ticket = _ticket_to_location.find(ticket_id); if (_ticket == _ticket_to_location.end()) { @@ -234,13 +224,12 @@ _ticket_to_location.erase(_ticket); } private: - std::mutex _mtx; std::unordered_map _current_location_reservations; std::unordered_map _ticket_to_location; }; -///A queue that allowss removal of an item based on its value. +///A queue that allows removal of an item based on its value. template class ItemQueue { @@ -290,7 +279,7 @@ class ItemQueue /// This class enqueues items based on how old a request is. /// The basic idea is that we maintain a queue for every resource. As requests -/// come in we simultaneously add them to every queue which cam beserviced. +/// come in we simultaneously add them to every queue which can be serviced. /// Once a resource becomes free we call `service_next_in_queue` for said resource. /// When we service the next item in the queue we remove it from all other queues. class ServiceQueueManager diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index a4fafd227..0615838c7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -61,4 +61,4 @@ class ReservationManager }; } } -#endif \ No newline at end of file +#endif diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index c8e301bae..dc0f20e02 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1737,6 +1737,91 @@ bool RobotContext::_has_ticket() const return _reservation_mgr.has_ticket(); } +//============================================================================== +std::vector + RobotContext::_find_and_sort_parking_spots( + const bool same_floor, const std::vector &waypoint_ids) const +{ + std::vector + final_result; + // Retrieve nav graph + const auto& graph = navigation_graph(); + + // Get current location + auto current_location = location(); + if (current_location.size() == 0) + { + // Could not localize should probably log an error + RCLCPP_ERROR(node()->get_logger(), "Unable to localize."); + return final_result; + } + + // In the event that no specific waypoint ids are used we look for parking spots + std::vector parking_spots; + if (waypoint_ids.size() == 0) + { + for (std::size_t i = 0; i < graph.num_waypoints(); ++i) + { + const auto& wp = graph.get_waypoint(i); + if (wp.is_parking_spot()) + { + parking_spots.push_back(i); + } + } + } + + const auto& waypoints = (waypoint_ids.size() == 0) ? + parking_spots : waypoint_ids; + + + // Order wait points by the distance from the destination. + std::vector> + waitpoints_order; + for (auto wp_idx: waypoints) + { + if (same_floor) + { + const auto& wp = graph.get_waypoint(wp_idx); + + // Check if same map. If not don't consider location. This is to ensure + // the robot does not try to board a lift. + if (wp.get_map_name() != map()) + { + RCLCPP_INFO( + node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + map().c_str()); + continue; + } + } + auto result = planner()->quickest_path(current_location, wp_idx); + if (!result.has_value()) + { + continue; + } + + rmf_traffic::agv::Plan::Goal goal(wp_idx); + waitpoints_order.emplace_back(result->cost(), goal); + + } + + //Sort waiting points + std::sort(waitpoints_order.begin(), waitpoints_order.end(), + [](const std::tuple& a, + const std::tuple& b) + { + return std::get<0>(a) < std::get<0>(b); + }); + + + for (auto &[_, waitpoint]: waitpoints_order) + { + final_result.push_back(waitpoint); + } + return final_result; +} //============================================================================== void RobotContext::_handle_mutex_group_manual_release( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 555a79a99..89605672b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -772,6 +772,14 @@ class RobotContext /// Set if the parking spot manager is used or not void _set_parking_spot_manager(const bool enabled); + /// Find all available spots from a list of graph waypoints + /// it an empty list is provided then we use the navgraph to + /// infer parking spotsotherwise we will always rely on + std::vector + _find_and_sort_parking_spots(const bool same_floor, + const std::vector &waypoint_ids=std::vector()) + const; + /// Set if the parking spot manager is used or not bool _parking_spot_manager_enabled(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 1bf017e49..1d4e9f156 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -200,52 +200,8 @@ auto EmergencyPullover::Active::make( // Use chope node to retrieve the best emergency location. active->_chosen_goal = std::nullopt; - // Find spots on same floor and order them by distance - auto current_location = active->_context->location(); - if (current_location.size() == 0) - { - // Could not localize should probably log an error and move into a - // retry timer of some form. - return active; - } - std::vector> - waitpoints_order; - for (std::size_t wp_idx = 0; - wp_idx < active->_context->navigation_graph().num_waypoints(); wp_idx++) - { - const auto wp = active->_context->navigation_graph().get_waypoint( - wp_idx); - - // Wait at parking spot and check its on same floor. - if (!wp.is_parking_spot() || - wp.get_map_name() != active->_context->map()) - { - continue; - } - - auto result = - active->_context->planner()->quickest_path(current_location, wp_idx); - if (!result.has_value()) - { - continue; - } - - rmf_traffic::agv::Plan::Goal goal(wp_idx); - waitpoints_order.emplace_back(result->cost(), goal); - } - - std::sort(waitpoints_order.begin(), waitpoints_order.end(), - [](const std::tuple& a, - const std::tuple& b) - { - return std::get<0>(a) < std::get<0>(b); - }); - - std::vector potential_waitpoints; - for (auto &[_, wp]: waitpoints_order) - { - potential_waitpoints.push_back(wp); - } + const auto potential_waitpoints = + std::move(active->_context->_find_and_sort_parking_spots(true)); RCLCPP_INFO(active->_context->node()->get_logger(), "Creating Chope negotiator"); @@ -423,81 +379,81 @@ void EmergencyPullover::Active::_find_plan() } else { - _find_path_service = std::make_shared( - _context->planner(), _context->location(), *_chosen_goal, - _context->schedule()->snapshot(), _context->itinerary().id(), - _context->profile(), - std::chrono::seconds(5)); - - const auto start_name = wp_name(*_context); - const auto goal_name = wp_name(*_context, *_chosen_goal); - - _plan_subscription = rmf_rxcpp::make_job( - _find_path_service) - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [w = weak_from_this(), start_name, goal_name, goal = *_chosen_goal]( - const services::FindPath::Result& result) - { - const auto self = w.lock(); - if (!self) - return; + _find_path_service = std::make_shared( + _context->planner(), _context->location(), *_chosen_goal, + _context->schedule()->snapshot(), _context->itinerary().id(), + _context->profile(), + std::chrono::seconds(5)); - if (!result) + const auto start_name = wp_name(*_context); + const auto goal_name = wp_name(*_context, *_chosen_goal); + + _plan_subscription = rmf_rxcpp::make_job( + _find_path_service) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [w = weak_from_this(), start_name, goal_name, goal = *_chosen_goal]( + const services::FindPath::Result& result) { - // The planner could not find a way to reach the goal - self->_state->update_status(Status::Error); - self->_state->update_log().error( - "Failed to find a plan to move from [" - + start_name + "] to [" + goal_name + "]. Will retry soon."); + const auto self = w.lock(); + if (!self) + return; - // Reset the chosen goal in case this goal has become impossible to - // reach - self->_chosen_goal = std::nullopt; - self->_execution = std::nullopt; - self->_schedule_retry(); + if (!result) + { + // The planner could not find a way to reach the goal + self->_state->update_status(Status::Error); + self->_state->update_log().error( + "Failed to find a plan to move from [" + + start_name + "] to [" + goal_name + "]. Will retry soon."); - self->_context->worker() - .schedule([update = self->_update](const auto&) { update(); }); + // Reset the chosen goal in case this goal has become impossible to + // reach + self->_chosen_goal = std::nullopt; + self->_execution = std::nullopt; + self->_schedule_retry(); - return; - } + self->_context->worker() + .schedule([update = self->_update](const auto&) { update(); }); - self->_state->update_status(Status::Underway); - self->_state->update_log().info( - "Found a plan to move from [" - + start_name + "] to [" + goal_name + "]"); + return; + } + self->_state->update_status(Status::Underway); + self->_state->update_log().info( + "Found a plan to move from [" + + start_name + "] to [" + goal_name + "]"); - std::vector no_future_projections; - auto full_itinerary = project_itinerary( - *result, no_future_projections, - *self->_context->planner()); - self->_execute_plan( - self->_context->itinerary().assign_plan_id(), - *std::move(result), - std::move(full_itinerary)); + std::vector no_future_projections; + auto full_itinerary = project_itinerary( + *result, no_future_projections, + *self->_context->planner()); - self->_find_path_service = nullptr; - self->_retry_timer = nullptr; - }); + self->_execute_plan( + self->_context->itinerary().assign_plan_id(), + *std::move(result), + std::move(full_itinerary)); - _find_path_timeout = _context->node()->try_create_wall_timer( - std::chrono::seconds(10), - [ - weak_service = _find_path_service->weak_from_this(), - weak_self = weak_from_this() - ]() - { - if (const auto service = weak_service.lock()) - service->interrupt(); + self->_find_path_service = nullptr; + self->_retry_timer = nullptr; + }); - if (const auto self = weak_self.lock()) - self->_find_path_timeout = nullptr; - }); + _find_path_timeout = _context->node()->try_create_wall_timer( + std::chrono::seconds(10), + [ + weak_service = _find_path_service->weak_from_this(), + weak_self = weak_from_this() + ]() + { + if (const auto service = weak_service.lock()) + service->interrupt(); - _update(); + if (const auto self = weak_self.lock()) + self->_find_path_timeout = nullptr; + }); + + _update(); } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 69da6444b..ba2e300f1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -285,7 +285,7 @@ auto GoToPlace::Active::make( RCLCPP_INFO(active->_context->node()->get_logger(), "Creating Chope negotiator"); - active->_chope_client = chope::ChopeNodeNegotiator::make( + active->_chope_client = std::move(chope::ChopeNodeNegotiator::make( active->_context, active->_description.one_of(), active->_description.prefer_same_map(), @@ -301,7 +301,7 @@ auto GoToPlace::Active::make( self->_on_chope_node_allocate_waitpoint(goal); } } - ); + )); }; return active; @@ -467,10 +467,10 @@ std::string wp_name(const agv::RobotContext& context) //============================================================================== std::optional GoToPlace::Active::_choose_goal( - bool only_same_map) + bool only_same_map) const { auto current_location = _context->location(); - auto graph = _context->navigation_graph(); + const auto& graph = _context->navigation_graph(); if (current_location.size() == 0) { //unable to get location. We should return some form of error stste. @@ -492,7 +492,6 @@ std::optional GoToPlace::Active::_choose_goal( { RCLCPP_INFO(_context->node()->get_logger(), "Requesting Chope Node For Time To Progress"); - } else { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index bdedbebc0..cec1cac7e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -106,22 +106,15 @@ class GoToPlace : public rmf_task_sequence::Event void kill() final; private: - enum class ReservationState - { - Pending=0, - Requested=1, - RecievedResponse=2 - }; - - ReservationState _current_reservation_state = ReservationState::Pending; Active(Description description); void _schedule_retry(); + // Chooses goal if the goal is on the same map. Otherwise it std::optional _choose_goal( - bool only_same_map); + bool only_same_map) const; void _find_plan(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index bf8cce8f7..4c78af3aa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -47,16 +47,11 @@ class ChopeNodeNegotiator: _reservation_ticket = _context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( _context->worker())) - .subscribe([w = - weak_from_this()](const std::shared_ptr + .subscribe([self = this](const std::shared_ptr &msg) { - const auto self = w.lock(); - if (!self) - return; - - RCLCPP_ERROR( + RCLCPP_INFO( self->_context->node()->get_logger(), "Got ticket issueing claim"); @@ -68,65 +63,24 @@ class ChopeNodeNegotiator: } self->_ticket = msg; + self->_waitpoints = self->_context->_find_and_sort_parking_spots(true); - // Pick the nearest location to wait - auto current_location = self->_context->location(); - if (current_location.size() == 0) + if (self->_waitpoints.size() == 0) { + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Got no waitpoints"); return; } - // Order wait points by the distance from the destination. - std::vector> - waitpoints_order; - for (std::size_t wp_idx = 0; - wp_idx < self->_context->navigation_graph().num_waypoints(); wp_idx++) - { - const auto wp = self->_context->navigation_graph().get_waypoint( - wp_idx); - - auto name = wp.name(); - if (name == nullptr) - { - RCLCPP_ERROR(self->_context->node()->get_logger(), - "Got a parking spot without a name." - "This parking spot will not be used by the reservation system."); - continue; - } - - // Wait at parking spot and check its on same floor. - if (!wp.is_parking_spot() || - wp.get_map_name() != self->_context->map()) - { - continue; - } - - auto result = - self->_context->planner()->quickest_path(current_location, wp_idx); - if (!result.has_value()) - { - continue; - } - - rmf_traffic::agv::Plan::Goal goal(wp_idx); - waitpoints_order.emplace_back(result->cost(), *name, goal); - } - - std::sort(waitpoints_order.begin(), waitpoints_order.end(), - [](const std::tuple& a, - const std::tuple& b) - { - return std::get<0>(a) < std::get<0>(b); - }); - // Immediately make claim cause we don't yet support flexible reservations. rmf_chope_msgs::msg::ClaimRequest claim_request; claim_request.ticket = *msg; - std::vector waitpoints; - for (auto &[_, waitpoint, waitpoint_goal]: waitpoints_order) + for (const auto &goal: self->_waitpoints) { - claim_request.wait_points.push_back(waitpoint); - self->_waitpoints.push_back(waitpoint_goal); + auto wp = + self->_context->navigation_graph().get_waypoint(goal.waypoint()); + claim_request.wait_points.push_back(*wp.name()); } self->_context->node()->claim_location_ticket()->publish(claim_request); RCLCPP_ERROR( @@ -138,14 +92,9 @@ class ChopeNodeNegotiator: _reservation_allocation = _context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( _context->worker())) - .subscribe([w = - weak_from_this()](const std::shared_ptr + .subscribe([self = this](const std::shared_ptr &msg) { - const auto self = w.lock(); - if (!self) - return; - if (!self->_ticket.has_value()) { return; @@ -211,13 +160,13 @@ class ChopeNodeNegotiator: selected_final_destination_cb, const std::function selected_waitpoint_cb) { - + RCLCPP_INFO(context->node()->get_logger(), + "Constructing chope negotiator"); auto negotiator = std::make_shared(context, goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); return negotiator; } - private: enum class ReservationState @@ -246,7 +195,7 @@ class ChopeNodeNegotiator: RCLCPP_INFO( _context->node()->get_logger(), - "Selecting a new go_to_place location from [%lu] choices for robot [%s]", + "Chope Negotiator: Selecting a new go_to_place location from [%lu] choices for robot [%s]", _goals.size(), _context->requester_id().c_str()); diff --git a/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp b/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp index 808d7d8b2..f63cbc0bb 100644 --- a/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp +++ b/rmf_fleet_adapter/test/services/test_FindEmergencyPullover.cpp @@ -15,7 +15,6 @@ * */ -#include #include #include #include @@ -128,8 +127,7 @@ SCENARIO("Emergency Pullover") auto pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, - rmf_traffic::agv::Plan::StartSet({start_0}), + planner, rmf_traffic::agv::Plan::StartSet({start_0}), database->snapshot(), p0.id(), std::make_shared(p0.description().profile())); @@ -166,8 +164,7 @@ SCENARIO("Emergency Pullover") // want to park at waypoint 7 in the absence of any other conflicts. pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, - rmf_traffic::agv::Plan::StartSet({start_1}), + planner, rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); @@ -208,8 +205,7 @@ SCENARIO("Emergency Pullover") pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, - rmf_traffic::agv::Plan::StartSet({start_1}), + planner, rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); @@ -265,8 +261,7 @@ SCENARIO("Emergency Pullover") auto pullover_service = std::make_shared< rmf_fleet_adapter::services::FindEmergencyPullover>( - planner, - rmf_traffic::agv::Plan::StartSet({start_1}), + planner, rmf_traffic::agv::Plan::StartSet({start_1}), database->snapshot(), p1.id(), std::make_shared(p1.description().profile())); From f8681314e21e7edf509bba110027cdf00f74acbb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Aug 2024 15:57:09 +0800 Subject: [PATCH 078/119] More style fixes Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 4 ++-- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 4 ++++ .../src/rmf_fleet_adapter/events/ExecutePlan.cpp | 2 -- .../rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp | 6 ------ 4 files changed, 6 insertions(+), 10 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index e0b764470..f71b43e5e 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -161,8 +161,8 @@ class CurrentState /// Tries to greedily allocate the lowest cost free spot given a list of potential /// parking spots. - /// \param[in] incoming_requests - Parking spot and cost of said parking spot - /// \param[in] ticket_id - Ticket which is being serviced + /// \param[in] incoming_requests - Parking spot and cost of said parking spot. + /// \param[in] ticket_id - Ticket which is being serviced. std::optional allocate_lowest_cost_free_spot( const std::vector& incoming_requests, const std::size_t ticket_id) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index dc0f20e02..cb2b334f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1799,6 +1799,10 @@ std::vector auto result = planner()->quickest_path(current_location, wp_idx); if (!result.has_value()) { + RCLCPP_INFO( + node()->get_logger(), + "No path found for waypoint #%lu", + wp_idx); continue; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 3a1c96b31..2e7c99a52 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -32,7 +32,6 @@ #include "../agv/internal_RobotUpdateHandle.hpp" #include -#include namespace rmf_fleet_adapter { namespace events { @@ -860,7 +859,6 @@ std::optional ExecutePlan::make( std::vector waypoints = plan.get_waypoints(); - auto waypoint_copy = waypoints; std::vector move_through; std::optional current_mutex_groups; std::unordered_set remaining_mutex_groups; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 4c78af3aa..a97278e58 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -242,12 +242,6 @@ class ChopeNodeNegotiator: wp_idx, result->cost()); - if (result->cost() < lowest_cost) - { - selected_idx = i; - lowest_cost = result->cost(); - } - auto name = wp.name(); if (name == nullptr) { From 7728eb726aa6648cfb9f551625a18dce7e58d945 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Aug 2024 15:57:43 +0800 Subject: [PATCH 079/119] More style fixes Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp index 70ca490ec..703349d35 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp @@ -1,4 +1,3 @@ - /* * Copyright (C) 2021 Open Source Robotics Foundation * From 7dcc41f65e8baffda43d4ba16a16a45fa1415dca Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Aug 2024 16:10:28 +0800 Subject: [PATCH 080/119] More style fixes Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 2 +- .../rmf_fleet_adapter/agv/RobotUpdateHandle.hpp | 4 ---- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 6 ++---- .../src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp | 13 ------------- .../events/internal_ChopeNegotiator.hpp | 2 +- 5 files changed, 4 insertions(+), 23 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index 097016c42..86a13e93b 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -568,7 +568,7 @@ class EasyFullControl::FleetConfiguration /// The minimum length that a lane should have. /// /// \param[in] use_parking_reservation - /// Whether ro use + /// Whether to use parking_reservations. FleetConfiguration( const std::string& fleet_name, std::optional> diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 23f62dd45..132c749c6 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -408,10 +408,6 @@ class RobotUpdateHandle /// By default this behavior is enabled. void enable_responsive_wait(bool value); - - /// Enable simple parking spot - void enable_simple_parkingspot_reservation(bool value); - /// If the robot is holding onto a session with a lift, release that session. void release_lift(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 89605672b..06486d4df 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -964,23 +964,21 @@ class RobotContext const rmf_fleet_msgs::msg::MutexGroupManualRelease& msg); std::unordered_map _requesting_mutex_groups; std::unordered_map _locked_mutex_groups; - rxcpp::subjects::subject _mutex_group_lock_subject; rxcpp::observable _mutex_group_lock_obs; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; - rclcpp::Subscription::SharedPtr _mutex_group_manual_release_sub; std::chrono::steady_clock::time_point _last_active_task_time; uint64_t _last_reservation_request_id; ReservationManager _reservation_mgr; + bool _use_parking_spot_reservations; + std::optional _final_lift_destination; std::unique_ptr _final_lift_destination_mutex = std::make_unique(); - - bool _use_parking_spot_reservations; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 2c9ffa02d..849f25ae4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -712,19 +712,6 @@ void RobotUpdateHandle::enable_responsive_wait(bool value) }); } -//============================================================================== -void RobotUpdateHandle::enable_simple_parkingspot_reservation(bool value) -{ - const auto context = _pimpl->get_context(); - if (!context) - return; - - context->worker().schedule( - [context, value](const auto&) - { - context->_set_parking_spot_manager(value); - }); -} //============================================================================== void RobotUpdateHandle::release_lift() diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index a97278e58..ac9bbf1b3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -298,4 +298,4 @@ class ChopeNodeNegotiator: } // namespace rmf_fleet_adapter } // namespace -#endif \ No newline at end of file +#endif From 730236987d9dadb27211df757ea87bb283c980da Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 16 Aug 2024 16:11:47 +0800 Subject: [PATCH 081/119] More style fixes Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 849f25ae4..761d11216 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -712,7 +712,6 @@ void RobotUpdateHandle::enable_responsive_wait(bool value) }); } - //============================================================================== void RobotUpdateHandle::release_lift() { From f8aad5d5149b80b45395870ccf35f35854a3b4e8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 19 Aug 2024 11:26:50 +0800 Subject: [PATCH 082/119] More refactoring Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 4 ++- .../rmf_fleet_adapter/agv/RobotContext.cpp | 30 ++++++------------- .../rmf_fleet_adapter/agv/RobotContext.hpp | 8 ++--- 3 files changed, 15 insertions(+), 27 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index f71b43e5e..e515d879f 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -154,6 +154,8 @@ class CurrentState { if (_current_location_reservations.count(location) == 0) { + std::cerr << "Got duplicate location [" << location + << "]" << std::endl; _current_location_reservations.emplace(location, LocationState {std::nullopt}); } @@ -179,7 +181,7 @@ class CurrentState positions[incoming_requests[i].location] = i; } - auto requests = std::move(incoming_requests); + auto requests = incoming_requests; std::sort(requests.begin(), requests.end()); for (std::size_t i = 0; i < requests.size(); i++) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index cb2b334f3..10e6f5740 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1740,7 +1740,7 @@ bool RobotContext::_has_ticket() const //============================================================================== std::vector RobotContext::_find_and_sort_parking_spots( - const bool same_floor, const std::vector &waypoint_ids) const + const bool same_floor) const { std::vector final_result; @@ -1756,32 +1756,20 @@ std::vector return final_result; } - // In the event that no specific waypoint ids are used we look for parking spots - std::vector parking_spots; - if (waypoint_ids.size() == 0) - { - for (std::size_t i = 0; i < graph.num_waypoints(); ++i) - { - const auto& wp = graph.get_waypoint(i); - if (wp.is_parking_spot()) - { - parking_spots.push_back(i); - } - } - } - - const auto& waypoints = (waypoint_ids.size() == 0) ? - parking_spots : waypoint_ids; - - // Order wait points by the distance from the destination. std::vector> waitpoints_order; - for (auto wp_idx: waypoints) + for (std::size_t wp_idx = 0; wp_idx < graph.num_waypoints(); ++wp_idx) { + const auto& wp = graph.get_waypoint(wp_idx); + + if (!wp.is_parking_spot()) + { + continue; + } + if (same_floor) { - const auto& wp = graph.get_waypoint(wp_idx); // Check if same map. If not don't consider location. This is to ensure // the robot does not try to board a lift. diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 06486d4df..d7f21e3b8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -772,12 +772,10 @@ class RobotContext /// Set if the parking spot manager is used or not void _set_parking_spot_manager(const bool enabled); - /// Find all available spots from a list of graph waypoints - /// it an empty list is provided then we use the navgraph to - /// infer parking spotsotherwise we will always rely on + /// Find all available spots. + /// \param[in] same_floor - if the parking spots should be on the same floor. std::vector - _find_and_sort_parking_spots(const bool same_floor, - const std::vector &waypoint_ids=std::vector()) + _find_and_sort_parking_spots(const bool same_floor) const; /// Set if the parking spot manager is used or not From 1af04f0f7673b09539ecd97faad2691b4d58f789 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 20 Aug 2024 12:13:04 +0800 Subject: [PATCH 083/119] Was logging the wrong thing Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index e515d879f..03104eb02 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -154,11 +154,14 @@ class CurrentState { if (_current_location_reservations.count(location) == 0) { - std::cerr << "Got duplicate location [" << location - << "]" << std::endl; _current_location_reservations.emplace(location, LocationState {std::nullopt}); } + else + { + std::cerr << "Got duplicate location [" << location + << "]" << std::endl; + } } /// Tries to greedily allocate the lowest cost free spot given a list of potential From 196bfdde9b342e3b10e10cdeb4e061da517ddade Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 21 Aug 2024 08:53:17 +0800 Subject: [PATCH 084/119] Update Readme Signed-off-by: Arjo Chakravarty --- rmf_chope_node/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmf_chope_node/README.md b/rmf_chope_node/README.md index a5db2e075..153431af7 100644 --- a/rmf_chope_node/README.md +++ b/rmf_chope_node/README.md @@ -1,6 +1,6 @@ # rmf\_chope\_node package -"Chope" is the very singaporean act of throwing an item on a table and marking it at yours before going to get your food. The goal of this package is to provide the same functionality in RMF. +"Chope" is the very Singaporean act of throwing an item on a table and marking it at yours before going to get your food. The goal of this package is to provide the same functionality in RMF. Before a robot goes to its next destination the fleet adapter asks the chope-node if its next destination is available. If the destination is available the robot will immediately proceed to the next destination, otherwise it will allocate a free parking spot for the robot to wait at till the next destination becomes available in a FIFO manner. If you need more advanced methods than FIFO, feel free to hack this package. To enable the use of this package you need to add the following to your fleet configuration yaml at the fleet level and run the chope node: ```yaml use_parking_reservations: True @@ -12,8 +12,7 @@ Some immediate limitations of the chope node is that it is limited to scenarios ## Expected Behaviour ### Basic queueing -You should never need to interact with this package directly. Rather, you can simply dispatch a task using the traditional methods. If you have `rmf_demos` installed you can run the -office world. We can start by commanding the `tinyRobot2` to go to the pantry. +You should never need to interact with this package directly. Rather, you can simply dispatch a task using the traditional methods. If you have `rmf_demos` installed you can run the office world. We can start by commanding the `tinyRobot2` to go to the pantry. ```bash ros2 run rmf_demos_tasks dispatch_go_to_place -p pantry -F tinyRobot -R tinyRobot2 --use_sim_time ``` @@ -44,7 +43,8 @@ If you have the same number or more waitpoints than robots you should never have ## Protocol Used Behind the Scenes -The chope node has no idea about the state of the graph it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace`. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleetadapter submits a `ReservationRequest` message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The chope node will issue a ticket for said request.When ready to proceed, send a claim message with the ticket and with the list of potential waiting points ordered by distance. The chope node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. +The chope node has no information about the state of the graph, it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace` event. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleet adapter submits a ReservationRequest message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The chope node will issue a ticket for said request. When ready to proceed, send a claim message with the ticket and list of potential waiting points ordered by distance. The chope node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. + ## Known Issues 1. At start up if there is no idle task, the chope node will not know where the robots are. It is advised to send 1 `GoToPlace` task for every robot that is added to the world. From da54f122215c49f2995e6536ec5c533a88986066 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 21 Aug 2024 15:06:22 +0800 Subject: [PATCH 085/119] Fix deadlock We now traverse the wait-graph instead of just releasing the next ticket. This should hopefully make things less buggy. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 79 +++++++++++-------- .../agv/ReservationManager.hpp | 7 -- .../events/EmergencyPullover.cpp | 2 +- .../events/internal_ChopeNegotiator.hpp | 29 ++++--- 4 files changed, 65 insertions(+), 52 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 03104eb02..bd306fd3e 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -137,7 +137,8 @@ struct LocationReq class CurrentState { public: - std::vector free_locations() + /// Get list of free locations + std::vector free_locations() const { std::vector locations; for (auto&[loc, state] : _current_location_reservations) @@ -206,7 +207,7 @@ class CurrentState } } - std::cerr << "Could not free space from any of: "; + std::cerr << "Could not find a free space from any of: "; for (auto c: requests) { std::cerr << c.location << ", "; } @@ -234,7 +235,7 @@ _ticket_to_location.erase(_ticket); }; -///A queue that allows removal of an item based on its value. +/// A queue that allows removal of an item based on its value. template class ItemQueue { @@ -292,6 +293,7 @@ class ServiceQueueManager std::unordered_map> resource_queues; public: + /// Service std::optional service_next_in_queue(const std::string& resource) { auto item = resource_queues[resource].front(); @@ -307,7 +309,8 @@ class ServiceQueueManager return item; } - void add_to_queue(std::size_t ticket, std::vector& resources) + void add_to_queue( + std::size_t ticket, std::vector& resources) { for(auto resource: resources) { @@ -492,38 +495,52 @@ class ChopeNode : public rclcpp::Node request->ticket.ticket_id); return; } + RCLCPP_INFO( + this->get_logger(), "Released ticket %lu, location %s is now free", + request->ticket.ticket_id, + released_location->c_str()); - auto next_item = queue_manager_.service_next_in_queue(released_location.value()); - if (!next_item.has_value()) + // Traverse waitgraph. + while(auto next_ticket = queue_manager_.service_next_in_queue(released_location.value())) { + // Release the ticket + released_location = current_state_.release(next_ticket.value()); + if (!released_location.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), "Could not find ticket %lu", + next_ticket.value()); + return; + } + + auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_ticket.value()], + next_ticket.value()); RCLCPP_INFO( - this->get_logger(), "Queue is now empty %s", - released_location->c_str()); - return; - } - // Will go to lowest - auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_item.value()], - next_item.value()); - RCLCPP_INFO( - this->get_logger(), "Found next item %lu on queue %s", - next_item.value(), - released_location.value().c_str()); + this->get_logger(), "Found next item %lu on queue %s", + next_ticket.value(), + released_location.value().c_str()); - if (!result.has_value()) - { - RCLCPP_ERROR( - this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the chope node's state and the", ticket); - return; + + if (!result.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the chope node's state and the", ticket); + return; + } + rmf_chope_msgs::msg::ReservationAllocation allocation; + allocation.satisfies_alternative = result.value(); + allocation.resource = requests_[next_ticket.value()][result.value()].location; + allocation.ticket = ticket_store_.get_existing_ticket(next_ticket.value()); + allocation.instruction_type = + rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", + allocation.resource.c_str(), next_ticket.value()); + allocation_pub_->publish(allocation); } - rmf_chope_msgs::msg::ReservationAllocation allocation; - allocation.satisfies_alternative = result.value(); - allocation.resource = requests_[next_item.value()][result.value()].location; - allocation.ticket = ticket_store_.get_existing_ticket(next_item.value()); - allocation.instruction_type = - rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - allocation.resource.c_str(), next_item.value()); - allocation_pub_->publish(allocation); + RCLCPP_INFO( + this->get_logger(), "Queue is now empty %s", + released_location->c_str()); + return; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index 0615838c7..f8248e2d5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -24,13 +24,6 @@ namespace rmf_fleet_adapter { namespace agv { -enum class ReservationState -{ - AWAITING_REQUEST, - AWAITING_ALLOCATION, - AWAITING_TURN -}; - class ReservationManager { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 1d4e9f156..6abae49bb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -201,7 +201,7 @@ auto EmergencyPullover::Active::make( active->_chosen_goal = std::nullopt; const auto potential_waitpoints = - std::move(active->_context->_find_and_sort_parking_spots(true)); + active->_context->_find_and_sort_parking_spots(true); RCLCPP_INFO(active->_context->node()->get_logger(), "Creating Chope negotiator"); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index ac9bbf1b3..47aed5eb5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -53,7 +53,7 @@ class ChopeNodeNegotiator: RCLCPP_INFO( self->_context->node()->get_logger(), - "Got ticket issueing claim"); + "Chope: Got ticket issueing claim"); if (msg->header.request_id != self->_reservation_id || msg->header.robot_name != self->_context->name() @@ -69,7 +69,7 @@ class ChopeNodeNegotiator: { RCLCPP_ERROR( self->_context->node()->get_logger(), - "Got no waitpoints"); + "Chope: Got no waitpoints"); return; } @@ -85,7 +85,7 @@ class ChopeNodeNegotiator: self->_context->node()->claim_location_ticket()->publish(claim_request); RCLCPP_ERROR( self->_context->node()->get_logger(), - "Claim issued"); + "Chope: Claim issued"); }); @@ -110,17 +110,21 @@ class ChopeNodeNegotiator: if (self->_current_reservation_state == ReservationState::Requested) { + std::unordered_set released_ticket_ids; while (auto allocation = self->_context->_release_resource()) { + if (released_ticket_ids.count(allocation->ticket.ticket_id) != 0) + { + continue; + } + released_ticket_ids.insert(allocation->ticket.ticket_id); rmf_chope_msgs::msg::ReleaseRequest msg; - std::stringstream str; - str << self->_context->location()[0].waypoint(); - str >> msg.location; msg.ticket = allocation->ticket; self->_context->node()->release_location()->publish(msg); - RCLCPP_ERROR( + RCLCPP_INFO( self->_context->node()->get_logger(), - "Releasing waypoint" + "Chope: Releasing waypoint for ticket %lu", + msg.ticket ); } } @@ -129,7 +133,7 @@ class ChopeNodeNegotiator: == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { RCLCPP_INFO( - self->_context->node()->get_logger(), "chope: Robot %s is going to final destination", + self->_context->node()->get_logger(), "Chope: Robot %s is going to final destination", self->_context->name().c_str()); self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> @@ -143,7 +147,7 @@ class ChopeNodeNegotiator: self->_selected_waitpoint_cb(self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative]); RCLCPP_INFO( - self->_context->node()->get_logger(), "chope: Robot %s is being asked to proceed to a to waitpoint", + self->_context->node()->get_logger(), "Chope: Robot %s is being asked to proceed to a to waitpoint", self->_context->name().c_str()); } }); @@ -188,7 +192,7 @@ class ChopeNodeNegotiator: //unable to get location. We should return some form of error stste. RCLCPP_ERROR( _context->node()->get_logger(), - "Robot [%s] can't get location", + "Chope: Robot [%s] can't get location", _context->requester_id().c_str()); return; } @@ -201,13 +205,12 @@ class ChopeNodeNegotiator: if (_current_reservation_state == ReservationState::Pending) { - // Select node + // Submit costs of each alternative rmf_chope_msgs::msg::FlexibleTimeRequest ftr; ftr.header.robot_name = _context->name(); ftr.header.fleet_name = _context->group(); ftr.header.request_id = _reservation_id; - auto lowest_cost = std::numeric_limits::infinity(); std::optional selected_idx; for (std::size_t i = 0; i < _goals.size(); ++i) { From eb9740695b08fcbccc2b9c0e93bc6f36c9b98efa Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 22 Aug 2024 00:04:02 +0800 Subject: [PATCH 086/119] Style fixes Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 38 ++----------------- .../events/internal_ChopeNegotiator.hpp | 2 +- 2 files changed, 4 insertions(+), 36 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index bd306fd3e..015209286 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -45,35 +45,6 @@ struct std::hash }; -/// Useful for identifying which tickets belong to which robots. -struct RobotIdentifier -{ - std::string robot_name; - std::string fleet_name; - - bool operator==(const RobotIdentifier& other) const - { - return robot_name == other.robot_name && fleet_name == other.fleet_name; - } -}; - - -template<> -struct std::hash -{ - std::size_t operator()(const RobotIdentifier& header) const - { - using std::size_t; - using std::hash; - using std::string; - - - return (hash()(header.robot_name) - ^ (hash()(header.fleet_name) << 1)) >> 1; - } -}; - - /// Ticket generation class for book keeping purposes. Will eventually overflow. /// Ticket id 0 does not exist and is useful for making emergency claims. /// Ticket ids are mapped across multiple fleets. @@ -87,10 +58,7 @@ class TicketStore rmf_chope_msgs::msg::Ticket ticket; ticket.header = request_header; ticket.ticket_id = _last_issued_ticket_id; - RobotIdentifier robot_identifier { - request_header.robot_name, - request_header.fleet_name - }; + _ticket_to_header.emplace(_last_issued_ticket_id, request_header); _last_issued_ticket_id++; return ticket; @@ -350,7 +318,7 @@ class ChopeNode : public rclcpp::Node graph_subscription_ = this->create_subscription( "/nav_graphs", qos, - std::bind(&ChopeNode::recieved_graph, this, + std::bind(&ChopeNode::received_graph, this, std::placeholders::_1)); ticket_pub_ = this->create_publisher( @@ -368,7 +336,7 @@ class ChopeNode : public rclcpp::Node } private: - void recieved_graph( + void received_graph( const rmf_building_map_msgs::msg::Graph::ConstSharedPtr& graph_msg) { RCLCPP_INFO(this->get_logger(), "Got graph"); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 47aed5eb5..d715a59d1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -124,7 +124,7 @@ class ChopeNodeNegotiator: RCLCPP_INFO( self->_context->node()->get_logger(), "Chope: Releasing waypoint for ticket %lu", - msg.ticket + msg.ticket.ticket_id ); } } From 5dca4bb1e511fb86aae779d685501881b0bd25db Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 27 Aug 2024 16:19:27 +0800 Subject: [PATCH 087/119] Add support for getting lost Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 7 ++- .../events/internal_ChopeNegotiator.hpp | 61 +++++++++++++++++-- 2 files changed, 62 insertions(+), 6 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 015209286..37b237b08 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -408,8 +408,13 @@ class ChopeNode : public rclcpp::Node } // If we can't proceed immediately add the ticket to a queue. - RCLCPP_INFO(this->get_logger(), "Could not immediately service %lu, enqueing.", + RCLCPP_INFO(this->get_logger(), "Could not immediately service %lu, enqueing. Locations in ticket were:", request->ticket.ticket_id); + for (auto i = 0; i < requests_[request->ticket.ticket_id].size(); i++) + { + RCLCPP_INFO(this->get_logger(), "\t- %s", + requests_[request->ticket.ticket_id][i].location.c_str()); + } queue_manager_.add_to_queue(request->ticket.ticket_id, location_names); // Allocate a waitpoint by preference as given by Fleet Adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index d715a59d1..13493e891 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -133,11 +133,12 @@ class ChopeNodeNegotiator: == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { RCLCPP_INFO( - self->_context->node()->get_logger(), "Chope: Robot %s is going to final destination", - self->_context->name().c_str()); + self->_context->node()->get_logger(), "Chope: Robot %s is going to final destination %lu", + self->_context->name().c_str(), + self->_goals[self->_final_allocated_destination.value()->satisfies_alternative]); self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> - satisfies_alternative]); + satisfies_alternative].waypoint()); } if (msg->instruction_type @@ -147,11 +148,59 @@ class ChopeNodeNegotiator: self->_selected_waitpoint_cb(self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative]); RCLCPP_INFO( - self->_context->node()->get_logger(), "Chope: Robot %s is being asked to proceed to a to waitpoint", - self->_context->name().c_str()); + self->_context->node()->get_logger(), "Chope: Robot %s is being asked to proceed to a waitpoint %lu", + self->_context->name().c_str(), + self->_waitpoints[self->_final_allocated_destination.value()-> + satisfies_alternative].waypoint()); } }); + + auto current_location =_context->location(); + if (current_location.size() == 0) + { + using namespace std::literals::chrono_literals; + _retry_timer = _context->node()->create_wall_timer( + 500ms, [this, same_map]() + { + auto current_location = _context->location(); + if (current_location.size() != 0) + { + _retry_timer->cancel(); + } + else { + return; + } + for (std::size_t i = 0; i < _goals.size(); ++i) + { + if (_goals[i].waypoint() == current_location[0].waypoint()) + { + RCLCPP_ERROR(_context->node()->get_logger(), + "Already at goal no need to engage reservation system\n"); + _selected_final_destination_cb(_goals[_final_allocated_destination.value()-> + satisfies_alternative].waypoint()); + return; + } + } + RCLCPP_INFO(_context->node()->get_logger(), + "Sending chope request"); + make_request(same_map); + } + ); + return; + } + + for (std::size_t i = 0; i < _goals.size(); ++i) + { + if (_goals[i].waypoint() == current_location[0].waypoint()) + { + RCLCPP_ERROR(_context->node()->get_logger(), + "Already at goal no need to engage reservation system\n"); + _selected_final_destination_cb(_goals[_final_allocated_destination.value()-> + satisfies_alternative]); + return; + } + } RCLCPP_INFO(context->node()->get_logger(), "Sending chope request"); make_request(same_map); @@ -173,6 +222,8 @@ class ChopeNodeNegotiator: private: + rclcpp::TimerBase::SharedPtr _retry_timer; + enum class ReservationState { Pending=0, From 62446ab2249c00f5d10382d61e9578af8ab324fc Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 28 Aug 2024 10:50:04 +0800 Subject: [PATCH 088/119] Fix segfault Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 13493e891..d763c26dd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -177,8 +177,7 @@ class ChopeNodeNegotiator: { RCLCPP_ERROR(_context->node()->get_logger(), "Already at goal no need to engage reservation system\n"); - _selected_final_destination_cb(_goals[_final_allocated_destination.value()-> - satisfies_alternative].waypoint()); + _selected_final_destination_cb(_goals[i].waypoint()); return; } } @@ -196,8 +195,7 @@ class ChopeNodeNegotiator: { RCLCPP_ERROR(_context->node()->get_logger(), "Already at goal no need to engage reservation system\n"); - _selected_final_destination_cb(_goals[_final_allocated_destination.value()-> - satisfies_alternative]); + _selected_final_destination_cb(_goals[i]); return; } } From f424094d9efc2bc7a685b9b3b434b8dec3d9617f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 2 Sep 2024 10:31:32 +0800 Subject: [PATCH 089/119] Fixed patrol behaviour So my previous hypothesis about patrols was rubbish. It turns out there was an inconsitency within the chope node itself. The issue was that there were duplicates being added to the queue. This means that the chope node was deadlocking. I've addressed that with this fix. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 43 +++++++--- .../rmf_fleet_adapter/events/GoToPlace.cpp | 80 +++++++++++-------- .../events/internal_ChopeNegotiator.hpp | 2 +- 3 files changed, 81 insertions(+), 44 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 37b237b08..390041428 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -18,6 +18,7 @@ #include #include +#include const std::string ReservationRequestTopicName = "/rmf/reservations/request"; const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; @@ -68,10 +69,21 @@ class TicketStore { rmf_chope_msgs::msg::Ticket ticket; ticket.ticket_id = index; - ticket.header = _ticket_to_header[index]; + auto res = _ticket_to_header.find(index); + if (res != _ticket_to_header.end()) + ticket.header = _ticket_to_header[index]; return ticket; } + std::string debug_ticket(const std::size_t index) + { + auto ticket = get_existing_ticket(index); + std::stringstream ss; + ss << ticket.header.fleet_name << "/" << ticket.header.robot_name + << "(" << index << ")"; + return ss.str(); + } + std::unordered_map _ticket_to_header; std::size_t _last_issued_ticket_id = 1; @@ -212,6 +224,13 @@ class ItemQueue /// Time complexity: O(1) void add(T item) { + // Don't add the same item to the queue twice. + auto index = item_to_index.find(item); + if (index != item_to_index.end()) + { + return; + } + // index stores the order of the item in the queue. index_to_item[curr_index] = item; item_to_index[item] = curr_index; indices.insert(curr_index); @@ -401,8 +420,9 @@ class ChopeNode : public rclcpp::Node allocation.satisfies_alternative = result.value(); allocation.resource = requests_[request->ticket.ticket_id][result.value()].location; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - allocation.resource.c_str(), request->ticket.ticket_id); + RCLCPP_INFO(this->get_logger(), "Allocating %s to %s", + allocation.resource.c_str(), + ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); return; } @@ -443,8 +463,9 @@ class ChopeNode : public rclcpp::Node rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY; allocation.satisfies_alternative = waitpoint_result.value(); allocation.resource = wait_points[waitpoint_result.value()].location; - RCLCPP_INFO(this->get_logger(), "Allocating %s as waitpoint to %lu", - allocation.resource.c_str(), request->ticket.ticket_id); + RCLCPP_INFO(this->get_logger(), "Allocating %s as waitpoint to %s", + allocation.resource.c_str(), + ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); } else @@ -457,8 +478,8 @@ class ChopeNode : public rclcpp::Node const rmf_chope_msgs::msg::ReleaseRequest::ConstSharedPtr& request) { RCLCPP_INFO( - this->get_logger(), "Releasing ticket for %lu", - request->ticket.ticket_id); + this->get_logger(), "Releasing ticket for %s", + ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); auto ticket = request->ticket.ticket_id; auto released_location = current_state_.release(ticket); if (!released_location.has_value()) @@ -481,7 +502,7 @@ class ChopeNode : public rclcpp::Node if (!released_location.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Could not find ticket %lu", + this->get_logger(), "Could not find ticket %lu while traversing wait graph", next_ticket.value()); return; } @@ -506,8 +527,10 @@ class ChopeNode : public rclcpp::Node allocation.ticket = ticket_store_.get_existing_ticket(next_ticket.value()); allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %lu", - allocation.resource.c_str(), next_ticket.value()); + RCLCPP_INFO(this->get_logger(), "Allocating %s to %s as a result of %s leaving", + allocation.resource.c_str(), + ticket_store_.debug_ticket(next_ticket.value()).c_str(), + ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); } RCLCPP_INFO( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index ba2e300f1..0a86e44db 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -30,6 +30,37 @@ namespace rmf_fleet_adapter { namespace events { +//============================================================================== +std::string wp_name( + const agv::RobotContext& context, + const rmf_traffic::agv::Plan::Goal& goal) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& wp = g.get_waypoint(goal.waypoint()); + if (wp.name()) + return *wp.name(); + + return "#" + std::to_string(goal.waypoint()); +} + +//============================================================================== +std::string wp_name(const agv::RobotContext& context) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& locations = context.location(); + for (const auto& l : locations) + { + const auto& wp = g.get_waypoint(l.waypoint()); + if (wp.name()) + return *wp.name(); + } + + if (locations.empty()) + return ""; + + return "#" + std::to_string(locations.front().waypoint()); +} + //============================================================================== void GoToPlace::add(rmf_task_sequence::Event::Initializer& initializer) { @@ -202,8 +233,8 @@ auto GoToPlace::Active::make( RCLCPP_INFO( self->_context->node()->get_logger(), - "Goal selected %lu", - self->_chosen_goal.value().waypoint()); + "Goal selected %s", + wp_name(*self->_context, self->_chosen_goal.value()).c_str()); self->_find_plan(); } @@ -318,7 +349,9 @@ void GoToPlace::Active::_on_chope_node_allocate_final_destination( const rmf_traffic::agv::Plan::Goal& goal) { RCLCPP_INFO(_context->node()->get_logger(), - "Received final destination from chope node"); + "%s Received final destination %s from chope node", + _context->requester_id().c_str(), + wp_name(*_context, goal).c_str()); _is_final_destination = true; _chosen_goal = goal; @@ -434,36 +467,6 @@ void GoToPlace::Active::kill() _finished(); } -//============================================================================== -std::string wp_name( - const agv::RobotContext& context, - const rmf_traffic::agv::Plan::Goal& goal) -{ - const auto& g = context.planner()->get_configuration().graph(); - const auto& wp = g.get_waypoint(goal.waypoint()); - if (wp.name()) - return *wp.name(); - - return "#" + std::to_string(goal.waypoint()); -} - -//============================================================================== -std::string wp_name(const agv::RobotContext& context) -{ - const auto& g = context.planner()->get_configuration().graph(); - const auto& locations = context.location(); - for (const auto& l : locations) - { - const auto& wp = g.get_waypoint(l.waypoint()); - if (wp.name()) - return *wp.name(); - } - - if (locations.empty()) - return ""; - - return "#" + std::to_string(locations.front().waypoint()); -} //============================================================================== std::optional GoToPlace::Active::_choose_goal( @@ -587,6 +590,17 @@ void GoToPlace::Active::_find_plan() return; } + if (_context->location().size() == 0) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] is lost retrying.", + _context->requester_id().c_str()); + + _schedule_retry(); + return; + } + _state->update_status(Status::Underway); const auto start_name = wp_name(*_context); const auto goal_name = wp_name(*_context, *_chosen_goal); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index d763c26dd..dcbb59162 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -135,7 +135,7 @@ class ChopeNodeNegotiator: RCLCPP_INFO( self->_context->node()->get_logger(), "Chope: Robot %s is going to final destination %lu", self->_context->name().c_str(), - self->_goals[self->_final_allocated_destination.value()->satisfies_alternative]); + self->_goals[self->_final_allocated_destination.value()->satisfies_alternative].waypoint()); self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); From ca75f7eb766e3a06c9ace40d43cd3db4169f7ceb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 17 Sep 2024 10:19:30 +0800 Subject: [PATCH 090/119] Support cancellation Also remove duplicated headers. Make chopenode depend on fleet adapter for names of topics. Signed-off-by: Arjo Chakravarty --- rmf_chope_node/CMakeLists.txt | 6 ++- rmf_chope_node/package.xml | 1 + rmf_chope_node/src/main.cpp | 35 +++++++--------- .../rmf_fleet_adapter/events/GoToPlace.cpp | 9 ++++ .../events/internal_ChopeNegotiator.hpp | 42 +++++++++++-------- 5 files changed, 55 insertions(+), 38 deletions(-) diff --git a/rmf_chope_node/CMakeLists.txt b/rmf_chope_node/CMakeLists.txt index 2cfac3fa2..b5d81510b 100644 --- a/rmf_chope_node/CMakeLists.txt +++ b/rmf_chope_node/CMakeLists.txt @@ -10,9 +10,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmf_chope_msgs REQUIRED) find_package(rmf_building_map_msgs REQUIRED) +find_package(rmf_fleet_adapter REQUIRED) add_executable(queue_manager src/main.cpp) -ament_target_dependencies(queue_manager rclcpp rmf_chope_msgs rmf_building_map_msgs) +ament_target_dependencies(queue_manager + rclcpp rmf_chope_msgs rmf_building_map_msgs) +target_link_libraries(queue_manager rmf_fleet_adapter::rmf_fleet_adapter) +target_include_directories(queue_manager PRIVATE ${rmf_fleet_adapter_INCLUDE_DIRS}) install(TARGETS queue_manager diff --git a/rmf_chope_node/package.xml b/rmf_chope_node/package.xml index 06425cc46..31a3ad423 100644 --- a/rmf_chope_node/package.xml +++ b/rmf_chope_node/package.xml @@ -14,6 +14,7 @@ rmf_building_map_msgs rmf_chope_msgs + rmf_fleet_adapter ament_cmake diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 390041428..2ab6533a2 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -1,10 +1,8 @@ #include "rmf_chope_msgs/msg/flexible_time_request.hpp" #include "rmf_chope_msgs/msg/claim_request.hpp" #include "rclcpp/rclcpp.hpp" -#include -#include -#include -#include + + #include #include @@ -14,20 +12,14 @@ #include #include #include -#include -#include +#include #include #include +#include +#include -const std::string ReservationRequestTopicName = "/rmf/reservations/request"; -const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; -const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; -const std::string ReservationAllocationTopicName = - "/rmf/reservations/allocation"; -const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; - - +using namespace rmf_fleet_adapter; /// C++-isms template<> struct std::hash @@ -133,6 +125,12 @@ class CurrentState void add_location(std::string location) { + if (location.empty()) + { + std::cerr << "Got an empty location name. Make sure all locations" + << " are set correctly" << std::endl; + return; + } if (_current_location_reservations.count(location) == 0) { _current_location_reservations.emplace(location, @@ -150,7 +148,7 @@ class CurrentState /// \param[in] incoming_requests - Parking spot and cost of said parking spot. /// \param[in] ticket_id - Ticket which is being serviced. std::optional allocate_lowest_cost_free_spot( - const std::vector& incoming_requests, + std::vector requests, const std::size_t ticket_id) { if (_ticket_to_location.count(ticket_id) != 0) @@ -160,12 +158,11 @@ class CurrentState } std::unordered_map positions; - for (std::size_t i = 0; i < incoming_requests.size(); ++i) + for (std::size_t i = 0; i < requests.size(); ++i) { - positions[incoming_requests[i].location] = i; + positions[requests[i].location] = i; } - auto requests = incoming_requests; std::sort(requests.begin(), requests.end()); for (std::size_t i = 0; i < requests.size(); i++) { @@ -430,7 +427,7 @@ class ChopeNode : public rclcpp::Node // If we can't proceed immediately add the ticket to a queue. RCLCPP_INFO(this->get_logger(), "Could not immediately service %lu, enqueing. Locations in ticket were:", request->ticket.ticket_id); - for (auto i = 0; i < requests_[request->ticket.ticket_id].size(); i++) + for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); i++) { RCLCPP_INFO(this->get_logger(), "\t- %s", requests_[request->ticket.ticket_id][i].location.c_str()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 0a86e44db..367ac1ba7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -455,6 +455,11 @@ void GoToPlace::Active::cancel() _stop_and_clear(); _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); + + if (_context->_parking_spot_manager_enabled()) + { + _chope_client->force_release(); + } _finished(); } @@ -464,6 +469,10 @@ void GoToPlace::Active::kill() _stop_and_clear(); _state->update_status(Status::Killed); _state->update_log().info("Received signal to kill"); + if (_context->_parking_spot_manager_enabled()) + { + _chope_client->force_release(); + } _finished(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index dcbb59162..6cb46b4b3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -110,23 +110,7 @@ class ChopeNodeNegotiator: if (self->_current_reservation_state == ReservationState::Requested) { - std::unordered_set released_ticket_ids; - while (auto allocation = self->_context->_release_resource()) - { - if (released_ticket_ids.count(allocation->ticket.ticket_id) != 0) - { - continue; - } - released_ticket_ids.insert(allocation->ticket.ticket_id); - rmf_chope_msgs::msg::ReleaseRequest msg; - msg.ticket = allocation->ticket; - self->_context->node()->release_location()->publish(msg); - RCLCPP_INFO( - self->_context->node()->get_logger(), - "Chope: Releasing waypoint for ticket %lu", - msg.ticket.ticket_id - ); - } + self->force_release(); } if (msg->instruction_type @@ -203,6 +187,28 @@ class ChopeNodeNegotiator: "Sending chope request"); make_request(same_map); } + + void force_release() + { + std::unordered_set released_ticket_ids; + while (auto allocation = _context->_release_resource()) + { + if (released_ticket_ids.count(allocation->ticket.ticket_id) != 0) + { + continue; + } + released_ticket_ids.insert(allocation->ticket.ticket_id); + rmf_chope_msgs::msg::ReleaseRequest msg; + msg.ticket = allocation->ticket; + _context->node()->release_location()->publish(msg); + RCLCPP_INFO( + _context->node()->get_logger(), + "Chope: Releasing waypoint for ticket %lu", + msg.ticket.ticket_id + ); + } + } + static std::shared_ptr make( std::shared_ptr context, const std::vector goals, @@ -220,7 +226,7 @@ class ChopeNodeNegotiator: private: - rclcpp::TimerBase::SharedPtr _retry_timer; + rclcpp::TimerBase::SharedPtr _retry_timer; enum class ReservationState { From 06afb597c73617426ac7d1e6424145011f1ba213 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 17 Sep 2024 11:21:13 +0800 Subject: [PATCH 091/119] More Refactors Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/RobotContext.hpp | 3 -- .../events/EmergencyPullover.cpp | 42 +++++-------------- .../rmf_fleet_adapter/events/GoToPlace.cpp | 32 +------------- .../events/internal_Utilities.cpp | 38 +++++++++++++++++ .../events/internal_Utilities.hpp | 35 ++++++++++++++++ 5 files changed, 84 insertions(+), 66 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.hpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 7142ef896..c26cff348 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -47,8 +47,6 @@ #include "Node.hpp" #include "../Reporting.hpp" #include "ReservationManager.hpp" -#include -#include #include @@ -829,7 +827,6 @@ class RobotContext self->_publish_mutex_group_requests(); }); - context->_mutex_group_manual_release_sub = context->_node->create_subscription< rmf_fleet_msgs::msg::MutexGroupManualRelease>( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 6abae49bb..88ca70f0c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -24,41 +24,11 @@ #include #include "../project_itinerary.hpp" +#include "internal_Utilities.hpp" + namespace rmf_fleet_adapter { namespace events { -namespace { -//============================================================================== -std::string wp_name( - const agv::RobotContext& context, - const rmf_traffic::agv::Plan::Goal& goal) -{ - const auto& g = context.planner()->get_configuration().graph(); - const auto& wp = g.get_waypoint(goal.waypoint()); - if (wp.name()) - return *wp.name(); - - return "#" + std::to_string(goal.waypoint()); -} - -//============================================================================== -std::string wp_name(const agv::RobotContext& context) -{ - const auto& g = context.planner()->get_configuration().graph(); - const auto& locations = context.location(); - for (const auto& l : locations) - { - const auto& wp = g.get_waypoint(l.waypoint()); - if (wp.name()) - return *wp.name(); - } - - if (locations.empty()) - return ""; - - return "#" + std::to_string(locations.front().waypoint()); -} -} //============================================================================== class EmergencyPulloverDescription @@ -293,6 +263,10 @@ void EmergencyPullover::Active::cancel() _execution = std::nullopt; _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); + if (_context->_parking_spot_manager_enabled()) + { + _chope_client->force_release(); + } _finished(); } @@ -302,6 +276,10 @@ void EmergencyPullover::Active::kill() _execution = std::nullopt; _state->update_status(Status::Killed); _state->update_log().info("Received signal to kill"); + if (_context->_parking_spot_manager_enabled()) + { + _chope_client->force_release(); + } _finished(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 367ac1ba7..db14c3fc7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -18,6 +18,7 @@ #include "GoToPlace.hpp" #include "../project_itinerary.hpp" #include "internal_ChopeNegotiator.hpp" +#include "internal_Utilities.hpp" #include "PerformAction.hpp" #include @@ -30,37 +31,6 @@ namespace rmf_fleet_adapter { namespace events { -//============================================================================== -std::string wp_name( - const agv::RobotContext& context, - const rmf_traffic::agv::Plan::Goal& goal) -{ - const auto& g = context.planner()->get_configuration().graph(); - const auto& wp = g.get_waypoint(goal.waypoint()); - if (wp.name()) - return *wp.name(); - - return "#" + std::to_string(goal.waypoint()); -} - -//============================================================================== -std::string wp_name(const agv::RobotContext& context) -{ - const auto& g = context.planner()->get_configuration().graph(); - const auto& locations = context.location(); - for (const auto& l : locations) - { - const auto& wp = g.get_waypoint(l.waypoint()); - if (wp.name()) - return *wp.name(); - } - - if (locations.empty()) - return ""; - - return "#" + std::to_string(locations.front().waypoint()); -} - //============================================================================== void GoToPlace::add(rmf_task_sequence::Event::Initializer& initializer) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp new file mode 100644 index 000000000..5924225dc --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp @@ -0,0 +1,38 @@ +#include "internal_Utilities.hpp" + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +std::string wp_name( + const agv::RobotContext& context, + const rmf_traffic::agv::Plan::Goal& goal) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& wp = g.get_waypoint(goal.waypoint()); + if (wp.name()) + return *wp.name(); + + return "#" + std::to_string(goal.waypoint()); +} + +//============================================================================== +std::string wp_name(const agv::RobotContext& context) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& locations = context.location(); + for (const auto& l : locations) + { + const auto& wp = g.get_waypoint(l.waypoint()); + if (wp.name()) + return *wp.name(); + } + + if (locations.empty()) + return ""; + + return "#" + std::to_string(locations.front().waypoint()); +} + +} +} \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.hpp new file mode 100644 index 000000000..ea5420bba --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2024 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 SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_UTILITIES_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_UTILITIES_HPP + +#include "../agv/RobotContext.hpp" + +namespace rmf_fleet_adapter { +namespace events { + +/// Get waypoint namegiven goal +std::string wp_name( + const agv::RobotContext& context, + const rmf_traffic::agv::Plan::Goal& goal); + +///Get robot's current waypoint name +std::string wp_name(const agv::RobotContext& context); +} +} + +#endif From aff7fcb8e1c95d5c55cd10bf6007d43fc54cd566 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 17 Sep 2024 11:35:07 +0800 Subject: [PATCH 092/119] Address namespacing issue Signed-off-by: Arjo Chakravarty --- .../include/rmf_fleet_adapter/StandardNames.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 36ea51653..3eb2af9c6 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -76,12 +76,12 @@ const std::string MutexGroupStatesTopicName = "mutex_group_states"; const std::string MutexGroupManualReleaseTopicName = "mutex_group_manual_release"; -const std::string ReservationRequestTopicName = "/rmf/reservations/request"; -const std::string ReservationResponseTopicName = "/rmf/reservations/tickets"; -const std::string ReservationClaimTopicName = "/rmf/reservations/claim"; +const std::string ReservationRequestTopicName = "rmf/reservations/request"; +const std::string ReservationResponseTopicName = "rmf/reservations/tickets"; +const std::string ReservationClaimTopicName = "rmf/reservations/claim"; const std::string ReservationAllocationTopicName = - "/rmf/reservations/allocation"; -const std::string ReservationReleaseTopicName = "/rmf/reservations/release"; + "rmf/reservations/allocation"; +const std::string ReservationReleaseTopicName = "rmf/reservations/release"; const uint64_t Unclaimed = (uint64_t)(-1); From 33a7930537e0b4fcafa35c5e61cbc0b73c27dff4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 17 Sep 2024 11:40:30 +0800 Subject: [PATCH 093/119] Style Signed-off-by: Arjo Chakravarty --- rmf_chope_node/src/main.cpp | 70 ++++++++++++++++++++++--------------- 1 file changed, 42 insertions(+), 28 deletions(-) diff --git a/rmf_chope_node/src/main.cpp b/rmf_chope_node/src/main.cpp index 2ab6533a2..9d9655bbb 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_chope_node/src/main.cpp @@ -72,7 +72,7 @@ class TicketStore auto ticket = get_existing_ticket(index); std::stringstream ss; ss << ticket.header.fleet_name << "/" << ticket.header.robot_name - << "(" << index << ")"; + << "(" << index << ")"; return ss.str(); } @@ -128,7 +128,7 @@ class CurrentState if (location.empty()) { std::cerr << "Got an empty location name. Make sure all locations" - << " are set correctly" << std::endl; + << " are set correctly" << std::endl; return; } if (_current_location_reservations.count(location) == 0) @@ -139,7 +139,7 @@ class CurrentState else { std::cerr << "Got duplicate location [" << location - << "]" << std::endl; + << "]" << std::endl; } } @@ -185,7 +185,8 @@ class CurrentState } std::cerr << "Could not find a free space from any of: "; - for (auto c: requests) { + for (auto c: requests) + { std::cerr << c.location << ", "; } std::cerr << "\n"; @@ -202,7 +203,7 @@ class CurrentState } auto location = _ticket->second; _current_location_reservations[location].ticket = std::nullopt; -_ticket_to_location.erase(_ticket); + _ticket_to_location.erase(_ticket); return {location}; } @@ -296,7 +297,7 @@ class ServiceQueueManager void add_to_queue( std::size_t ticket, std::vector& resources) { - for(auto resource: resources) + for (auto resource: resources) { resource_queues[resource].add(ticket); } @@ -364,7 +365,7 @@ class ChopeNode : public rclcpp::Node //TODO(arjoc) make this configure-able if (param.name == "is_parking_spot" && param.value_bool) { - current_state_.add_location( graph_msg->vertices[i].name); + current_state_.add_location(graph_msg->vertices[i].name); } } } @@ -415,7 +416,8 @@ class ChopeNode : public rclcpp::Node allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; allocation.satisfies_alternative = result.value(); - allocation.resource = requests_[request->ticket.ticket_id][result.value()].location; + allocation.resource = + requests_[request->ticket.ticket_id][result.value()].location; RCLCPP_INFO(this->get_logger(), "Allocating %s to %s", allocation.resource.c_str(), @@ -425,9 +427,11 @@ class ChopeNode : public rclcpp::Node } // If we can't proceed immediately add the ticket to a queue. - RCLCPP_INFO(this->get_logger(), "Could not immediately service %lu, enqueing. Locations in ticket were:", + RCLCPP_INFO( + this->get_logger(), "Could not immediately service %lu, enqueing. Locations in ticket were:", request->ticket.ticket_id); - for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); i++) + for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); + i++) { RCLCPP_INFO(this->get_logger(), "\t- %s", requests_[request->ticket.ticket_id][i].location.c_str()); @@ -449,8 +453,9 @@ class ChopeNode : public rclcpp::Node } waitpoints_[request->ticket.ticket_id] = wait_points; - auto waitpoint_result = current_state_.allocate_lowest_cost_free_spot(wait_points, - request->ticket.ticket_id); + auto waitpoint_result = current_state_.allocate_lowest_cost_free_spot( + wait_points, + request->ticket.ticket_id); if (waitpoint_result.has_value()) { rmf_chope_msgs::msg::ReservationAllocation allocation; @@ -461,13 +466,15 @@ class ChopeNode : public rclcpp::Node allocation.satisfies_alternative = waitpoint_result.value(); allocation.resource = wait_points[waitpoint_result.value()].location; RCLCPP_INFO(this->get_logger(), "Allocating %s as waitpoint to %s", - allocation.resource.c_str(), - ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); + allocation.resource.c_str(), + ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); } else { - RCLCPP_ERROR(this->get_logger(), "Could not allocate a waiting point for robots from %lu waitpoints", wait_points.size()); + RCLCPP_ERROR( + this->get_logger(), "Could not allocate a waiting point for robots from %lu waitpoints", + wait_points.size()); } } @@ -492,7 +499,8 @@ class ChopeNode : public rclcpp::Node released_location->c_str()); // Traverse waitgraph. - while(auto next_ticket = queue_manager_.service_next_in_queue(released_location.value())) + while (auto next_ticket = + queue_manager_.service_next_in_queue(released_location.value())) { // Release the ticket released_location = current_state_.release(next_ticket.value()); @@ -504,36 +512,42 @@ class ChopeNode : public rclcpp::Node return; } - auto result = current_state_.allocate_lowest_cost_free_spot(requests_[next_ticket.value()], - next_ticket.value()); + auto result = + current_state_.allocate_lowest_cost_free_spot(requests_[next_ticket. + value()], + next_ticket.value()); RCLCPP_INFO( - this->get_logger(), "Found next item %lu on queue %s", - next_ticket.value(), - released_location.value().c_str()); + this->get_logger(), "Found next item %lu on queue %s", + next_ticket.value(), + released_location.value().c_str()); if (!result.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the chope node's state and the", ticket); + this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the chope node's state and the", + ticket); return; } rmf_chope_msgs::msg::ReservationAllocation allocation; allocation.satisfies_alternative = result.value(); - allocation.resource = requests_[next_ticket.value()][result.value()].location; - allocation.ticket = ticket_store_.get_existing_ticket(next_ticket.value()); + allocation.resource = + requests_[next_ticket.value()][result.value()].location; + allocation.ticket = + ticket_store_.get_existing_ticket(next_ticket.value()); allocation.instruction_type = rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %s as a result of %s leaving", + RCLCPP_INFO( + this->get_logger(), "Allocating %s to %s as a result of %s leaving", allocation.resource.c_str(), ticket_store_.debug_ticket(next_ticket.value()).c_str(), ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); } RCLCPP_INFO( - this->get_logger(), "Queue is now empty %s", - released_location->c_str()); - return; + this->get_logger(), "Queue is now empty %s", + released_location->c_str()); + return; } From da01d5dd929ef060fcecabb678822f59600f3aef Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 17 Sep 2024 12:45:10 +0800 Subject: [PATCH 094/119] Style Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 18 +-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 +- .../events/EmergencyPullover.cpp | 7 +- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 2 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 65 ++++---- .../events/internal_ChopeNegotiator.hpp | 147 ++++++++++-------- 6 files changed, 131 insertions(+), 112 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index abe403262..1fbce6a55 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1757,11 +1757,11 @@ bool RobotContext::_has_ticket() const //============================================================================== std::vector - RobotContext::_find_and_sort_parking_spots( - const bool same_floor) const +RobotContext::_find_and_sort_parking_spots( + const bool same_floor) const { std::vector - final_result; + final_result; // Retrieve nav graph const auto& graph = navigation_graph(); @@ -1776,7 +1776,7 @@ std::vector // Order wait points by the distance from the destination. std::vector> - waitpoints_order; + waitpoints_order; for (std::size_t wp_idx = 0; wp_idx < graph.num_waypoints(); ++wp_idx) { const auto& wp = graph.get_waypoint(wp_idx); @@ -1806,9 +1806,9 @@ std::vector if (!result.has_value()) { RCLCPP_INFO( - node()->get_logger(), - "No path found for waypoint #%lu", - wp_idx); + node()->get_logger(), + "No path found for waypoint #%lu", + wp_idx); continue; } @@ -1820,13 +1820,13 @@ std::vector //Sort waiting points std::sort(waitpoints_order.begin(), waitpoints_order.end(), [](const std::tuple& a, - const std::tuple& b) + const std::tuple& b) { return std::get<0>(a) < std::get<0>(b); }); - for (auto &[_, waitpoint]: waitpoints_order) + for (auto&[_, waitpoint]: waitpoints_order) { final_result.push_back(waitpoint); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index c26cff348..931127572 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -765,8 +765,8 @@ class RobotContext /// Find all available spots. /// \param[in] same_floor - if the parking spots should be on the same floor. std::vector - _find_and_sort_parking_spots(const bool same_floor) - const; + _find_and_sort_parking_spots(const bool same_floor) + const; /// Set if the parking spot manager is used or not bool _parking_spot_manager_enabled(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 88ca70f0c..adf0917f8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -292,7 +292,7 @@ void EmergencyPullover::Active::_find_plan() _state->update_status(Status::Underway); _state->update_log().info("Searching for an emergency pullover"); - if(!_context->_parking_spot_manager_enabled()) + if (!_context->_parking_spot_manager_enabled()) { _find_pullover_service = std::make_shared( _context->emergency_planner(), _context->location(), @@ -355,7 +355,8 @@ void EmergencyPullover::Active::_find_plan() _update(); } - else { + else + { _find_path_service = std::make_shared( _context->planner(), _context->location(), *_chosen_goal, @@ -431,7 +432,7 @@ void EmergencyPullover::Active::_find_plan() self->_find_path_timeout = nullptr; }); - _update(); + _update(); } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 2e7c99a52..1be96dd78 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -519,7 +519,7 @@ std::optional search_for_lift_group( auto final_lift_destination = agv::RobotUpdateHandle::LiftDestination::Implementation::make( - lift_name, lift_end->destination()); + lift_name, lift_end->destination()); auto category = "Take [lift:" + lift_name + "] to [floor:" + lift_end->destination() + "]"; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index db14c3fc7..9e0ad849a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -279,7 +279,8 @@ auto GoToPlace::Active::make( // just proceed to find plans as is. active->_find_plan(); } - else { + else + { // This is the parking spot manager. active->_is_final_destination = false; active->_chosen_goal = std::nullopt; @@ -287,23 +288,27 @@ auto GoToPlace::Active::make( RCLCPP_INFO(active->_context->node()->get_logger(), "Creating Chope negotiator"); active->_chope_client = std::move(chope::ChopeNodeNegotiator::make( - active->_context, - active->_description.one_of(), - active->_description.prefer_same_map(), - [w = active->weak_from_this()]( const rmf_traffic::agv::Plan::Goal& goal) { - if (auto self = w.lock()) - { - self->_on_chope_node_allocate_final_destination(goal); - } - }, - [w = active->weak_from_this()]( const rmf_traffic::agv::Plan::Goal& goal) { - if (auto self = w.lock()) - { - self->_on_chope_node_allocate_waitpoint(goal); - } - } + active->_context, + active->_description.one_of(), + active->_description.prefer_same_map(), + [w = + active->weak_from_this()](const rmf_traffic::agv::Plan::Goal& goal) + { + if (auto self = w.lock()) + { + self->_on_chope_node_allocate_final_destination(goal); + } + }, + [w = + active->weak_from_this()](const rmf_traffic::agv::Plan::Goal& goal) + { + if (auto self = w.lock()) + { + self->_on_chope_node_allocate_waitpoint(goal); + } + } )); - }; + } return active; } @@ -316,7 +321,7 @@ auto GoToPlace::Active::state() const -> ConstStatePtr //============================================================================== void GoToPlace::Active::_on_chope_node_allocate_final_destination( - const rmf_traffic::agv::Plan::Goal& goal) + const rmf_traffic::agv::Plan::Goal& goal) { RCLCPP_INFO(_context->node()->get_logger(), "%s Received final destination %s from chope node", @@ -330,7 +335,7 @@ void GoToPlace::Active::_on_chope_node_allocate_final_destination( //============================================================================== void GoToPlace::Active::_on_chope_node_allocate_waitpoint( - const rmf_traffic::agv::Plan::Goal& goal) + const rmf_traffic::agv::Plan::Goal& goal) { RCLCPP_INFO(_context->node()->get_logger(), "Received waitpoint from chope node"); @@ -502,7 +507,8 @@ std::optional GoToPlace::Active::_choose_goal( } // Find distance to said point - auto result = _context->planner()->quickest_path(current_location, wp_idx); + auto result = + _context->planner()->quickest_path(current_location, wp_idx); if (result.has_value()) { RCLCPP_INFO( @@ -571,7 +577,7 @@ void GoToPlace::Active::_find_plan() if (_context->location().size() == 0) { - RCLCPP_ERROR( + RCLCPP_ERROR( _context->node()->get_logger(), "Robot [%s] is lost retrying.", _context->requester_id().c_str()); @@ -748,10 +754,11 @@ void GoToPlace::Active::_execute_plan( _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), - _assign_id, _state, _update, [&](){ + _assign_id, _state, _update, [&]() + { RCLCPP_INFO( - _context->node()->get_logger(), - "Chope: Finished execution"); + _context->node()->get_logger(), + "Chope: Finished execution"); _stop_and_clear(); _finished(); }, _tail_period); @@ -761,10 +768,11 @@ void GoToPlace::Active::_execute_plan( _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), - _assign_id, _state, _update, [&](){ + _assign_id, _state, _update, [&]() + { RCLCPP_INFO( - _context->node()->get_logger(), - "Chope: Reached waitpoint"); + _context->node()->get_logger(), + "Chope: Reached waitpoint"); _reached_waitpoint = true; }, _tail_period); @@ -814,7 +822,8 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( { if (self->_chosen_goal.has_value()) { - self->_execute_plan(plan_id, plan, std::move(itinerary), self->_chosen_goal.value()); + self->_execute_plan(plan_id, plan, std::move( + itinerary), self->_chosen_goal.value()); return self->_context->itinerary().version(); } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp index 6cb46b4b3..caeafac09 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp @@ -27,7 +27,7 @@ namespace chope { /// This class implements the protocol for negotiating a spot with the "chope" /// node. The chope node maintains a list of spots which are free. -class ChopeNodeNegotiator: +class ChopeNodeNegotiator : public std::enable_shared_from_this { public: @@ -36,7 +36,7 @@ class ChopeNodeNegotiator: const std::vector goals, const bool same_map, const std::function - selected_final_destination_cb, + selected_final_destination_cb, const std::function selected_waitpoint_cb) { _context = context; @@ -46,54 +46,56 @@ class ChopeNodeNegotiator: _reservation_id = _context->last_reservation_request_id(); _reservation_ticket = _context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( - _context->worker())) + _context->worker())) .subscribe([self = this](const std::shared_ptr - &msg) - { + & msg) + { - RCLCPP_INFO( - self->_context->node()->get_logger(), - "Chope: Got ticket issueing claim"); + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Chope: Got ticket issueing claim"); - if (msg->header.request_id != self->_reservation_id - || msg->header.robot_name != self->_context->name() - || msg->header.fleet_name != self->_context->group()) - { - return; - } + if (msg->header.request_id != self->_reservation_id + || msg->header.robot_name != self->_context->name() + || msg->header.fleet_name != self->_context->group()) + { + return; + } - self->_ticket = msg; - self->_waitpoints = self->_context->_find_and_sort_parking_spots(true); + self->_ticket = msg; + self->_waitpoints = self->_context->_find_and_sort_parking_spots( + true); - if (self->_waitpoints.size() == 0) - { - RCLCPP_ERROR( - self->_context->node()->get_logger(), - "Chope: Got no waitpoints"); - return; - } + if (self->_waitpoints.size() == 0) + { + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Chope: Got no waitpoints"); + return; + } - // Immediately make claim cause we don't yet support flexible reservations. - rmf_chope_msgs::msg::ClaimRequest claim_request; - claim_request.ticket = *msg; - for (const auto &goal: self->_waitpoints) - { - auto wp = + // Immediately make claim cause we don't yet support flexible reservations. + rmf_chope_msgs::msg::ClaimRequest claim_request; + claim_request.ticket = *msg; + for (const auto& goal: self->_waitpoints) + { + auto wp = self->_context->navigation_graph().get_waypoint(goal.waypoint()); - claim_request.wait_points.push_back(*wp.name()); - } - self->_context->node()->claim_location_ticket()->publish(claim_request); - RCLCPP_ERROR( - self->_context->node()->get_logger(), - "Chope: Claim issued"); - }); + claim_request.wait_points.push_back(*wp.name()); + } + self->_context->node()->claim_location_ticket()->publish( + claim_request); + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Chope: Claim issued"); + }); _reservation_allocation = _context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( _context->worker())) .subscribe([self = this](const std::shared_ptr - &msg) + & msg) { if (!self->_ticket.has_value()) { @@ -114,33 +116,38 @@ class ChopeNodeNegotiator: } if (msg->instruction_type - == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { RCLCPP_INFO( - self->_context->node()->get_logger(), "Chope: Robot %s is going to final destination %lu", - self->_context->name().c_str(), - self->_goals[self->_final_allocated_destination.value()->satisfies_alternative].waypoint()); - self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; - self->_selected_final_destination_cb(self->_goals[self->_final_allocated_destination.value()-> + self->_context->node()->get_logger(), + "Chope: Robot %s is going to final destination %lu", + self->_context->name().c_str(), + self->_goals[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); + self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; + self->_selected_final_destination_cb(self->_goals[self-> + _final_allocated_destination.value()-> + satisfies_alternative].waypoint()); } if (msg->instruction_type - == rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) + == rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) { self->_current_reservation_state = ReservationState::ReceivedResponseProceedWaitPoint; - self->_selected_waitpoint_cb(self->_waitpoints[self->_final_allocated_destination.value()-> - satisfies_alternative]); + self->_selected_waitpoint_cb(self->_waitpoints[self-> + _final_allocated_destination.value()-> + satisfies_alternative]); RCLCPP_INFO( - self->_context->node()->get_logger(), "Chope: Robot %s is being asked to proceed to a waitpoint %lu", - self->_context->name().c_str(), - self->_waitpoints[self->_final_allocated_destination.value()-> + self->_context->node()->get_logger(), + "Chope: Robot %s is being asked to proceed to a waitpoint %lu", + self->_context->name().c_str(), + self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); } }); - auto current_location =_context->location(); + auto current_location = _context->location(); if (current_location.size() == 0) { using namespace std::literals::chrono_literals; @@ -152,22 +159,23 @@ class ChopeNodeNegotiator: { _retry_timer->cancel(); } - else { + else + { return; } for (std::size_t i = 0; i < _goals.size(); ++i) + { + if (_goals[i].waypoint() == current_location[0].waypoint()) { - if (_goals[i].waypoint() == current_location[0].waypoint()) - { - RCLCPP_ERROR(_context->node()->get_logger(), - "Already at goal no need to engage reservation system\n"); - _selected_final_destination_cb(_goals[i].waypoint()); - return; - } + RCLCPP_ERROR(_context->node()->get_logger(), + "Already at goal no need to engage reservation system\n"); + _selected_final_destination_cb(_goals[i].waypoint()); + return; } - RCLCPP_INFO(_context->node()->get_logger(), - "Sending chope request"); - make_request(same_map); + } + RCLCPP_INFO(_context->node()->get_logger(), + "Sending chope request"); + make_request(same_map); } ); return; @@ -214,13 +222,13 @@ class ChopeNodeNegotiator: const std::vector goals, const bool same_map, const std::function - selected_final_destination_cb, + selected_final_destination_cb, const std::function selected_waitpoint_cb) { RCLCPP_INFO(context->node()->get_logger(), "Constructing chope negotiator"); auto negotiator = std::make_shared(context, - goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); + goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); return negotiator; } @@ -237,7 +245,6 @@ class ChopeNodeNegotiator: }; - void make_request(bool only_same_map) { auto current_location = _context->location(); @@ -304,8 +311,8 @@ class ChopeNodeNegotiator: if (name == nullptr) { RCLCPP_ERROR(_context->node()->get_logger(), - "Got a parking spot without a name." - "This parking spot will not be used by the reservation system."); + "Got a parking spot without a name." + "This parking spot will not be used by the reservation system."); continue; } @@ -339,8 +346,10 @@ class ChopeNodeNegotiator: ReservationState _current_reservation_state = ReservationState::Pending; std::shared_ptr _context; - std::function _selected_waitpoint_cb; - std::function _selected_final_destination_cb; + std::function + _selected_waitpoint_cb; + std::function + _selected_final_destination_cb; rmf_rxcpp::subscription_guard _reservation_ticket; rmf_rxcpp::subscription_guard _reservation_allocation; @@ -348,7 +357,7 @@ class ChopeNodeNegotiator: std::optional> _ticket{std:: nullopt}; std::optional> - _final_allocated_destination{std::nullopt}; + _final_allocated_destination{std::nullopt}; std::vector _goals; std::vector _waitpoints; From 704d1406612496fa263e7df3d5f127a727be24c5 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 24 Sep 2024 08:39:07 +0000 Subject: [PATCH 095/119] Fix navigation parameters Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index d24ff9060..039e92493 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -381,10 +381,10 @@ std::shared_ptr Adapter::add_easy_fleet( tf_dict, config.strict_lanes(), config.default_responsive_wait(), + config.use_parking_reservation_system(), config.default_max_merge_waypoint_distance(), config.default_max_merge_lane_distance(), - config.default_min_lane_length(), - config.use_parking_reservation_system()); + config.default_min_lane_length()); } //============================================================================== From 32f216044f24812a56c88f06a6cc6866de955150 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 25 Sep 2024 13:02:09 +0800 Subject: [PATCH 096/119] Mega Re-name As per a discussion with @mxgrey we are avoiding the use of the word "chope". Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/package.xml | 2 +- .../src/rmf_fleet_adapter/agv/Node.hpp | 24 ++--- .../agv/ReservationManager.hpp | 8 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 4 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 +- .../events/EmergencyPullover.cpp | 10 +- .../events/EmergencyPullover.hpp | 4 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 30 +++--- .../rmf_fleet_adapter/events/GoToPlace.hpp | 8 +- ...hpp => internal_ReservationNegotiator.hpp} | 64 +++++------ .../CMakeLists.txt | 6 +- .../README.md | 12 +-- .../package.xml | 4 +- .../src/main.cpp | 102 +++++++++--------- rmf_traffic_ros2/CMakeLists.txt | 6 +- rmf_traffic_ros2/package.xml | 2 +- 16 files changed, 144 insertions(+), 146 deletions(-) rename rmf_fleet_adapter/src/rmf_fleet_adapter/events/{internal_ChopeNegotiator.hpp => internal_ReservationNegotiator.hpp} (84%) rename {rmf_chope_node => rmf_reservation_node}/CMakeLists.txt (83%) rename {rmf_chope_node => rmf_reservation_node}/README.md (51%) rename {rmf_chope_node => rmf_reservation_node}/package.xml (89%) rename {rmf_chope_node => rmf_reservation_node}/src/main.cpp (83%) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 1c409f65f..214a56fa6 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -21,7 +21,7 @@ rmf_api_msgs rmf_battery rmf_building_map_msgs - rmf_chope_msgs + rmf_reservation_msgs rmf_dispenser_msgs rmf_door_msgs rmf_fleet_msgs diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 81b0945f6..348b7d506 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -33,12 +33,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include @@ -143,30 +143,30 @@ class Node : public rmf_rxcpp::Transport using MutexGroupStatesObs = rxcpp::observable; const MutexGroupStatesObs& mutex_group_states() const; - using ReservationRequest = rmf_chope_msgs::msg::FlexibleTimeRequest; + using ReservationRequest = rmf_reservation_msgs::msg::FlexibleTimeRequest; using ReservationRequestPub = rclcpp::Publisher::SharedPtr; const ReservationRequestPub& location_requester() const; - using ReservationTicket = rmf_chope_msgs::msg::Ticket; + using ReservationTicket = rmf_reservation_msgs::msg::Ticket; using ReservationTicketObs = rxcpp::observable; const ReservationTicketObs& location_ticket_obs() const; - using ReservationClaim = rmf_chope_msgs::msg::ClaimRequest; + using ReservationClaim = rmf_reservation_msgs::msg::ClaimRequest; using ReservationClaimPub = rclcpp::Publisher::SharedPtr; const ReservationClaimPub& claim_location_ticket() const; - using ReservationAllocation = rmf_chope_msgs::msg::ReservationAllocation; + using ReservationAllocation = rmf_reservation_msgs::msg::ReservationAllocation; using ReservationAllocationObs = rxcpp::observable; const ReservationAllocationObs& allocated_claims_obs() const; - using ReservationRelease = rmf_chope_msgs::msg::ReleaseRequest; + using ReservationRelease = rmf_reservation_msgs::msg::ReleaseRequest; using ReservationReleasePub = rclcpp::Publisher::SharedPtr; const ReservationReleasePub& release_location() const; - using ReservationFreeSpotStatus = rmf_chope_msgs::msg::FreeParkingSpots; + using ReservationFreeSpotStatus = rmf_reservation_msgs::msg::FreeParkingSpots; using ReservationFreeSpotObs = rxcpp::observable; const ReservationFreeSpotObs& freespots_obs() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index f8248e2d5..b4f0e3f07 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -19,7 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__AGV__RESERVATION_MANAGER_HPP #include #include -#include +#include namespace rmf_fleet_adapter { namespace agv { @@ -28,12 +28,12 @@ namespace agv { class ReservationManager { public: - void add_ticket(const rmf_chope_msgs::msg::ReservationAllocation alloc) + void add_ticket(const rmf_reservation_msgs::msg::ReservationAllocation alloc) { allocations.push_front(alloc); } - std::optional release_ticket() + std::optional release_ticket() { if (allocations.size() <= 1) { @@ -50,7 +50,7 @@ class ReservationManager return allocations.size() != 0; } - std::deque allocations; + std::deque allocations; }; } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 1978e85f7..a1a2e491c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1732,7 +1732,7 @@ void RobotContext::_publish_mutex_group_requests() //============================================================================== void RobotContext::_set_allocated_destination( - const rmf_chope_msgs::msg::ReservationAllocation& ticket) + const rmf_reservation_msgs::msg::ReservationAllocation& ticket) { _reservation_mgr.add_ticket(ticket); } @@ -1750,7 +1750,7 @@ bool RobotContext::_parking_spot_manager_enabled() } //============================================================================== -std::optional +std::optional RobotContext::_release_resource() { return _reservation_mgr.release_ticket(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 931127572..a745b7d66 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -757,7 +757,7 @@ class RobotContext /// Set an allocated destination. void _set_allocated_destination( - const rmf_chope_msgs::msg::ReservationAllocation&); + const rmf_reservation_msgs::msg::ReservationAllocation&); /// Set if the parking spot manager is used or not void _set_parking_spot_manager(const bool enabled); @@ -772,7 +772,7 @@ class RobotContext bool _parking_spot_manager_enabled(); /// Release last resource that was acquired. - std::optional _release_resource(); + std::optional _release_resource(); /// Has ticket now bool _has_ticket() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index adf0917f8..ad1a4ca16 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -167,15 +167,15 @@ auto EmergencyPullover::Active::make( } else { - // Use chope node to retrieve the best emergency location. + // Use reservation node to retrieve the best emergency location. active->_chosen_goal = std::nullopt; const auto potential_waitpoints = active->_context->_find_and_sort_parking_spots(true); RCLCPP_INFO(active->_context->node()->get_logger(), - "Creating Chope negotiator"); - active->_chope_client = chope::ChopeNodeNegotiator::make( + "Creating reservation negotiator"); + active->_reservation_client = reservation::reservationNodeNegotiator::make( active->_context, potential_waitpoints, true, @@ -265,7 +265,7 @@ void EmergencyPullover::Active::cancel() _state->update_log().info("Received signal to cancel"); if (_context->_parking_spot_manager_enabled()) { - _chope_client->force_release(); + _reservation_client->force_release(); } _finished(); } @@ -278,7 +278,7 @@ void EmergencyPullover::Active::kill() _state->update_log().info("Received signal to kill"); if (_context->_parking_spot_manager_enabled()) { - _chope_client->force_release(); + _reservation_client->force_release(); } _finished(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp index 1e3ced509..0ca7ace05 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp @@ -23,7 +23,7 @@ #include "../services/FindEmergencyPullover.hpp" #include "../services/FindPath.hpp" -#include "internal_ChopeNegotiator.hpp" +#include "internal_ReservationNegotiator.hpp" #include "ExecutePlan.hpp" @@ -129,7 +129,7 @@ class EmergencyPullover : public rmf_task_sequence::Event rclcpp::TimerBase::SharedPtr _find_path_timeout; std::optional _chosen_goal; - std::shared_ptr _chope_client; + std::shared_ptr _reservation_client; bool _is_interrupted = false; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 9e0ad849a..74f3fd1e9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -17,7 +17,7 @@ #include "GoToPlace.hpp" #include "../project_itinerary.hpp" -#include "internal_ChopeNegotiator.hpp" +#include "internal_ReservationNegotiator.hpp" #include "internal_Utilities.hpp" #include "PerformAction.hpp" @@ -286,8 +286,8 @@ auto GoToPlace::Active::make( active->_chosen_goal = std::nullopt; RCLCPP_INFO(active->_context->node()->get_logger(), - "Creating Chope negotiator"); - active->_chope_client = std::move(chope::ChopeNodeNegotiator::make( + "Creating reservation negotiator"); + active->_reservation_client = std::move(reservation::reservationNodeNegotiator::make( active->_context, active->_description.one_of(), active->_description.prefer_same_map(), @@ -296,7 +296,7 @@ auto GoToPlace::Active::make( { if (auto self = w.lock()) { - self->_on_chope_node_allocate_final_destination(goal); + self->_on_reservation_node_allocate_final_destination(goal); } }, [w = @@ -304,7 +304,7 @@ auto GoToPlace::Active::make( { if (auto self = w.lock()) { - self->_on_chope_node_allocate_waitpoint(goal); + self->_on_reservation_node_allocate_waitpoint(goal); } } )); @@ -320,11 +320,11 @@ auto GoToPlace::Active::state() const -> ConstStatePtr } //============================================================================== -void GoToPlace::Active::_on_chope_node_allocate_final_destination( +void GoToPlace::Active::_on_reservation_node_allocate_final_destination( const rmf_traffic::agv::Plan::Goal& goal) { RCLCPP_INFO(_context->node()->get_logger(), - "%s Received final destination %s from chope node", + "%s Received final destination %s from reservation node", _context->requester_id().c_str(), wp_name(*_context, goal).c_str()); _is_final_destination = true; @@ -334,11 +334,11 @@ void GoToPlace::Active::_on_chope_node_allocate_final_destination( } //============================================================================== -void GoToPlace::Active::_on_chope_node_allocate_waitpoint( +void GoToPlace::Active::_on_reservation_node_allocate_waitpoint( const rmf_traffic::agv::Plan::Goal& goal) { RCLCPP_INFO(_context->node()->get_logger(), - "Received waitpoint from chope node"); + "Received waitpoint from reservation node"); _chosen_goal = goal; _find_plan(); } @@ -433,7 +433,7 @@ void GoToPlace::Active::cancel() if (_context->_parking_spot_manager_enabled()) { - _chope_client->force_release(); + _reservation_client->force_release(); } _finished(); } @@ -446,7 +446,7 @@ void GoToPlace::Active::kill() _state->update_log().info("Received signal to kill"); if (_context->_parking_spot_manager_enabled()) { - _chope_client->force_release(); + _reservation_client->force_release(); } _finished(); } @@ -478,11 +478,11 @@ std::optional GoToPlace::Active::_choose_goal( if (_context->_parking_spot_manager_enabled()) { RCLCPP_INFO(_context->node()->get_logger(), - "Requesting Chope Node For Time To Progress"); + "Requesting reservation Node For Time To Progress"); } else { - RCLCPP_INFO(_context->node()->get_logger(), "Skipping chope node."); + RCLCPP_INFO(_context->node()->get_logger(), "Skipping reservation node."); auto lowest_cost = std::numeric_limits::infinity(); std::optional selected_idx; for (std::size_t i = 0; i < _description.one_of().size(); ++i) @@ -758,7 +758,7 @@ void GoToPlace::Active::_execute_plan( { RCLCPP_INFO( _context->node()->get_logger(), - "Chope: Finished execution"); + "reservation: Finished execution"); _stop_and_clear(); _finished(); }, _tail_period); @@ -772,7 +772,7 @@ void GoToPlace::Active::_execute_plan( { RCLCPP_INFO( _context->node()->get_logger(), - "Chope: Reached waitpoint"); + "reservation: Reached waitpoint"); _reached_waitpoint = true; }, _tail_period); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index cec1cac7e..0e63b485e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -24,7 +24,7 @@ #include "../services/FindPath.hpp" #include "ExecutePlan.hpp" -#include "internal_ChopeNegotiator.hpp" +#include "internal_ReservationNegotiator.hpp" #include #include @@ -126,10 +126,10 @@ class GoToPlace : public rmf_task_sequence::Event void _stop_and_clear(); - void _on_chope_node_allocate_final_destination( + void _on_reservation_node_allocate_final_destination( const rmf_traffic::agv::Plan::Goal& goal); - void _on_chope_node_allocate_waitpoint( + void _on_reservation_node_allocate_waitpoint( const rmf_traffic::agv::Plan::Goal& goal); Negotiator::NegotiatePtr _respond( @@ -154,7 +154,7 @@ class GoToPlace : public rmf_task_sequence::Event rmf_rxcpp::subscription_guard _replan_request_subscription; rmf_rxcpp::subscription_guard _graph_change_subscription; - std::shared_ptr _chope_client; + std::shared_ptr _reservation_client; bool _is_interrupted = false; bool _is_final_destination = true; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNegotiator.hpp similarity index 84% rename from rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp rename to rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNegotiator.hpp index caeafac09..f9d8c3758 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ChopeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNegotiator.hpp @@ -15,23 +15,23 @@ * */ -#ifndef SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_CHOPE_NEGOTIATOR_HPP -#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_CHOPE_NEGOTIATOR_HPP +#ifndef SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_reservation_NEGOTIATOR_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_reservation_NEGOTIATOR_HPP #include "../agv/RobotContext.hpp" #include namespace rmf_fleet_adapter { -namespace chope { +namespace reservation { -/// This class implements the protocol for negotiating a spot with the "chope" -/// node. The chope node maintains a list of spots which are free. -class ChopeNodeNegotiator : - public std::enable_shared_from_this +/// This class implements the protocol for negotiating a spot with the "reservation" +/// node. The reservation node maintains a list of spots which are free. +class reservationNodeNegotiator : + public std::enable_shared_from_this { public: - ChopeNodeNegotiator( + reservationNodeNegotiator( std::shared_ptr context, const std::vector goals, const bool same_map, @@ -47,13 +47,13 @@ class ChopeNodeNegotiator : _reservation_ticket = _context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( _context->worker())) - .subscribe([self = this](const std::shared_ptr + .subscribe([self = this](const std::shared_ptr & msg) { RCLCPP_INFO( self->_context->node()->get_logger(), - "Chope: Got ticket issueing claim"); + "reservation: Got ticket issueing claim"); if (msg->header.request_id != self->_reservation_id || msg->header.robot_name != self->_context->name() @@ -70,12 +70,12 @@ class ChopeNodeNegotiator : { RCLCPP_ERROR( self->_context->node()->get_logger(), - "Chope: Got no waitpoints"); + "reservation: Got no waitpoints"); return; } // Immediately make claim cause we don't yet support flexible reservations. - rmf_chope_msgs::msg::ClaimRequest claim_request; + rmf_reservation_msgs::msg::ClaimRequest claim_request; claim_request.ticket = *msg; for (const auto& goal: self->_waitpoints) { @@ -87,14 +87,14 @@ class ChopeNodeNegotiator : claim_request); RCLCPP_ERROR( self->_context->node()->get_logger(), - "Chope: Claim issued"); + "reservation: Claim issued"); }); _reservation_allocation = _context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( _context->worker())) - .subscribe([self = this](const std::shared_ptr + .subscribe([self = this](const std::shared_ptr & msg) { if (!self->_ticket.has_value()) @@ -116,11 +116,11 @@ class ChopeNodeNegotiator : } if (msg->instruction_type - == rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + == rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { RCLCPP_INFO( self->_context->node()->get_logger(), - "Chope: Robot %s is going to final destination %lu", + "reservation: Robot %s is going to final destination %lu", self->_context->name().c_str(), self->_goals[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); @@ -131,7 +131,7 @@ class ChopeNodeNegotiator : } if (msg->instruction_type - == rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) + == rmf_reservation_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) { self->_current_reservation_state = ReservationState::ReceivedResponseProceedWaitPoint; self->_selected_waitpoint_cb(self->_waitpoints[self-> @@ -139,7 +139,7 @@ class ChopeNodeNegotiator : satisfies_alternative]); RCLCPP_INFO( self->_context->node()->get_logger(), - "Chope: Robot %s is being asked to proceed to a waitpoint %lu", + "reservationvationvationvation: Robot %s is being asked to proceed to a waitpoint %lu", self->_context->name().c_str(), self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); @@ -174,7 +174,7 @@ class ChopeNodeNegotiator : } } RCLCPP_INFO(_context->node()->get_logger(), - "Sending chope request"); + "Sending reservation request"); make_request(same_map); } ); @@ -192,7 +192,7 @@ class ChopeNodeNegotiator : } } RCLCPP_INFO(context->node()->get_logger(), - "Sending chope request"); + "Sending reservation request"); make_request(same_map); } @@ -206,18 +206,18 @@ class ChopeNodeNegotiator : continue; } released_ticket_ids.insert(allocation->ticket.ticket_id); - rmf_chope_msgs::msg::ReleaseRequest msg; + rmf_reservation_msgs::msg::ReleaseRequest msg; msg.ticket = allocation->ticket; _context->node()->release_location()->publish(msg); RCLCPP_INFO( _context->node()->get_logger(), - "Chope: Releasing waypoint for ticket %lu", + "reservation: Releasing waypoint for ticket %lu", msg.ticket.ticket_id ); } } - static std::shared_ptr make( + static std::shared_ptr make( std::shared_ptr context, const std::vector goals, const bool same_map, @@ -226,8 +226,8 @@ class ChopeNodeNegotiator : const std::function selected_waitpoint_cb) { RCLCPP_INFO(context->node()->get_logger(), - "Constructing chope negotiator"); - auto negotiator = std::make_shared(context, + "Constructing reservation negotiator"); + auto negotiator = std::make_shared(context, goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); return negotiator; } @@ -254,21 +254,21 @@ class ChopeNodeNegotiator : //unable to get location. We should return some form of error stste. RCLCPP_ERROR( _context->node()->get_logger(), - "Chope: Robot [%s] can't get location", + "reservation: Robot [%s] can't get location", _context->requester_id().c_str()); return; } RCLCPP_INFO( _context->node()->get_logger(), - "Chope Negotiator: Selecting a new go_to_place location from [%lu] choices for robot [%s]", + "reservationvationvation Negotiator: Selecting a new go_to_place location from [%lu] choices for robot [%s]", _goals.size(), _context->requester_id().c_str()); if (_current_reservation_state == ReservationState::Pending) { // Submit costs of each alternative - rmf_chope_msgs::msg::FlexibleTimeRequest ftr; + rmf_reservation_msgs::msg::FlexibleTimeRequest ftr; ftr.header.robot_name = _context->name(); ftr.header.fleet_name = _context->group(); ftr.header.request_id = _reservation_id; @@ -316,12 +316,12 @@ class ChopeNodeNegotiator : continue; } - rmf_chope_msgs::msg::FlexibleTimeReservationAlt alternative; + rmf_reservation_msgs::msg::FlexibleTimeReservationAlt alternative; alternative.resource_name = *name; alternative.cost = result->cost(); alternative.has_end = false; - rmf_chope_msgs::msg::StartTimeRange start; + rmf_reservation_msgs::msg::StartTimeRange start; start.earliest_start_time = _context->node()->get_clock()->now(); start.latest_start_time = start.earliest_start_time; start.has_earliest_start_time = true; @@ -354,9 +354,9 @@ class ChopeNodeNegotiator : rmf_rxcpp::subscription_guard _reservation_allocation; uint64_t _reservation_id = 0; - std::optional> _ticket{std:: + std::optional> _ticket{std:: nullopt}; - std::optional> + std::optional> _final_allocated_destination{std::nullopt}; std::vector _goals; diff --git a/rmf_chope_node/CMakeLists.txt b/rmf_reservation_node/CMakeLists.txt similarity index 83% rename from rmf_chope_node/CMakeLists.txt rename to rmf_reservation_node/CMakeLists.txt index b5d81510b..bc7667f44 100644 --- a/rmf_chope_node/CMakeLists.txt +++ b/rmf_reservation_node/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(rmf_chope_node) +project(rmf_reservation_node) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -8,13 +8,13 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rmf_chope_msgs REQUIRED) +find_package(rmf_reservation_msgs REQUIRED) find_package(rmf_building_map_msgs REQUIRED) find_package(rmf_fleet_adapter REQUIRED) add_executable(queue_manager src/main.cpp) ament_target_dependencies(queue_manager - rclcpp rmf_chope_msgs rmf_building_map_msgs) + rclcpp rmf_reservation_msgs rmf_building_map_msgs) target_link_libraries(queue_manager rmf_fleet_adapter::rmf_fleet_adapter) target_include_directories(queue_manager PRIVATE ${rmf_fleet_adapter_INCLUDE_DIRS}) diff --git a/rmf_chope_node/README.md b/rmf_reservation_node/README.md similarity index 51% rename from rmf_chope_node/README.md rename to rmf_reservation_node/README.md index 153431af7..95d910185 100644 --- a/rmf_chope_node/README.md +++ b/rmf_reservation_node/README.md @@ -1,13 +1,13 @@ -# rmf\_chope\_node package +# rmf\_reservation\_node package -"Chope" is the very Singaporean act of throwing an item on a table and marking it at yours before going to get your food. The goal of this package is to provide the same functionality in RMF. -Before a robot goes to its next destination the fleet adapter asks the chope-node if its next destination is available. If the destination is available the robot will immediately proceed to the next destination, otherwise it will allocate a free parking spot for the robot to wait at till the next destination becomes available in a FIFO manner. If you need more advanced methods than FIFO, feel free to hack this package. To enable the use of this package you need to add the following to your fleet configuration yaml at the fleet level and run the chope node: +The goal of this package is to provide a simple parking reservation node in RMF. +Before a robot goes to its next destination the fleet adapter asks the reservation-node if its next destination is available. If the destination is available the robot will immediately proceed to the next destination, otherwise it will allocate a free parking spot for the robot to wait at till the next destination becomes available in a FIFO manner. If you need more advanced methods than FIFO, feel free to hack this package. To enable the use of this package you need to add the following to your fleet configuration yaml at the fleet level and run the reservation node: ```yaml use_parking_reservations: True ``` We recommend disabling `responsive_wait` when you do so. An example is available in `rmf_demos`. -Some immediate limitations of the chope node is that it is limited to scenarios with a single navigation graph. Overcoming this limitation should not be too much work now that the basic infrastructure is in place. +Some immediate limitations of the reservation node is that it is limited to scenarios with a single navigation graph. Overcoming this limitation should not be too much work now that the basic infrastructure is in place. ## Expected Behaviour @@ -43,11 +43,11 @@ If you have the same number or more waitpoints than robots you should never have ## Protocol Used Behind the Scenes -The chope node has no information about the state of the graph, it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace` event. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleet adapter submits a ReservationRequest message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The chope node will issue a ticket for said request. When ready to proceed, send a claim message with the ticket and list of potential waiting points ordered by distance. The chope node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. +The reservation node has no information about the state of the graph, it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace` event. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleet adapter submits a ReservationRequest message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The reservation node will issue a ticket for said request. When ready to proceed, send a claim message with the ticket and list of potential waiting points ordered by distance. The reservation node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. ## Known Issues -1. At start up if there is no idle task, the chope node will not know where the robots are. It is advised to send 1 `GoToPlace` task for every robot that is added to the world. +1. At start up if there is no idle task, the reservation node will not know where the robots are. It is advised to send 1 `GoToPlace` task for every robot that is added to the world. ## Quality Declaration diff --git a/rmf_chope_node/package.xml b/rmf_reservation_node/package.xml similarity index 89% rename from rmf_chope_node/package.xml rename to rmf_reservation_node/package.xml index 31a3ad423..90597d79c 100644 --- a/rmf_chope_node/package.xml +++ b/rmf_reservation_node/package.xml @@ -1,7 +1,7 @@ - rmf_chope_node + rmf_reservation_node 0.0.0 Node that handles current state of parking spots. Arjo Chakravarty @@ -13,7 +13,7 @@ ament_lint_common rmf_building_map_msgs - rmf_chope_msgs + rmf_reservation_msgs rmf_fleet_adapter diff --git a/rmf_chope_node/src/main.cpp b/rmf_reservation_node/src/main.cpp similarity index 83% rename from rmf_chope_node/src/main.cpp rename to rmf_reservation_node/src/main.cpp index 9d9655bbb..8ce6c29bd 100644 --- a/rmf_chope_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -1,17 +1,14 @@ -#include "rmf_chope_msgs/msg/flexible_time_request.hpp" -#include "rmf_chope_msgs/msg/claim_request.hpp" #include "rclcpp/rclcpp.hpp" - - #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -22,9 +19,9 @@ using namespace rmf_fleet_adapter; /// C++-isms template<> -struct std::hash +struct std::hash { - std::size_t operator()(const rmf_chope_msgs::msg::RequestHeader& header) const + std::size_t operator()(const rmf_reservation_msgs::msg::RequestHeader& header) const { using std::size_t; using std::hash; @@ -45,10 +42,10 @@ class TicketStore { public: - rmf_chope_msgs::msg::Ticket get_new_ticket( - const rmf_chope_msgs::msg::RequestHeader& request_header) + rmf_reservation_msgs::msg::Ticket get_new_ticket( + const rmf_reservation_msgs::msg::RequestHeader& request_header) { - rmf_chope_msgs::msg::Ticket ticket; + rmf_reservation_msgs::msg::Ticket ticket; ticket.header = request_header; ticket.ticket_id = _last_issued_ticket_id; @@ -57,9 +54,9 @@ class TicketStore return ticket; } - rmf_chope_msgs::msg::Ticket get_existing_ticket(const std::size_t index) + rmf_reservation_msgs::msg::Ticket get_existing_ticket(const std::size_t index) { - rmf_chope_msgs::msg::Ticket ticket; + rmf_reservation_msgs::msg::Ticket ticket; ticket.ticket_id = index; auto res = _ticket_to_header.find(index); if (res != _ticket_to_header.end()) @@ -77,7 +74,7 @@ class TicketStore } std::unordered_map _ticket_to_header; + rmf_reservation_msgs::msg::RequestHeader> _ticket_to_header; std::size_t _last_issued_ticket_id = 1; }; @@ -244,9 +241,10 @@ class ItemQueue { return; } + auto idx = index->second; item_to_index.erase(item); - index_to_item.erase(index->second); - indices.erase(index->second); + index_to_item.erase(idx); + indices.erase(idx); } // Gives the most recent item in the queue. @@ -306,11 +304,11 @@ class ServiceQueueManager using namespace std::chrono_literals; -class ChopeNode : public rclcpp::Node +class reservationNode : public rclcpp::Node { public: - ChopeNode() - : Node("rmf_chope_node") + reservationNode() + : Node("rmf_reservation_node") { rclcpp::QoS qos(10); @@ -319,37 +317,37 @@ class ChopeNode : public rclcpp::Node qos = qos.transient_local(); request_subscription_ = - this->create_subscription( + this->create_subscription( ReservationRequestTopicName, qos, - std::bind(&ChopeNode::on_request, this, + std::bind(&reservationNode::on_request, this, std::placeholders::_1)); claim_subscription_ = - this->create_subscription( + this->create_subscription( ReservationClaimTopicName, qos, - std::bind(&ChopeNode::claim_request, this, + std::bind(&reservationNode::claim_request, this, std::placeholders::_1)); release_subscription_ = - this->create_subscription( + this->create_subscription( ReservationReleaseTopicName, qos, - std::bind(&ChopeNode::release, this, std::placeholders::_1)); + std::bind(&reservationNode::release, this, std::placeholders::_1)); graph_subscription_ = this->create_subscription( "/nav_graphs", qos, - std::bind(&ChopeNode::received_graph, this, + std::bind(&reservationNode::received_graph, this, std::placeholders::_1)); - ticket_pub_ = this->create_publisher( + ticket_pub_ = this->create_publisher( ReservationResponseTopicName, qos); allocation_pub_ = - this->create_publisher( + this->create_publisher( ReservationAllocationTopicName, qos); free_spot_pub_ = - this->create_publisher( + this->create_publisher( "/rmf/reservations/free_parking_spot", qos); timer_ = this->create_wall_timer(500ms, - std::bind(&ChopeNode::publish_free_spots, this)); + std::bind(&reservationNode::publish_free_spots, this)); } private: @@ -371,7 +369,7 @@ class ChopeNode : public rclcpp::Node } } void on_request( - const rmf_chope_msgs::msg::FlexibleTimeRequest::ConstSharedPtr& request) + const rmf_reservation_msgs::msg::FlexibleTimeRequest::ConstSharedPtr& request) { std::vector requests; @@ -392,7 +390,7 @@ class ChopeNode : public rclcpp::Node } void claim_request( - const rmf_chope_msgs::msg::ClaimRequest::ConstSharedPtr& request) + const rmf_reservation_msgs::msg::ClaimRequest::ConstSharedPtr& request) { // This logic is for the simplified queue-less version. @@ -410,11 +408,11 @@ class ChopeNode : public rclcpp::Node request->ticket.ticket_id); if (result.has_value()) { - rmf_chope_msgs::msg::ReservationAllocation allocation; + rmf_reservation_msgs::msg::ReservationAllocation allocation; allocation.ticket = ticket_store_.get_existing_ticket( request->ticket.ticket_id); allocation.instruction_type = - rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; allocation.satisfies_alternative = result.value(); allocation.resource = requests_[request->ticket.ticket_id][result.value()].location; @@ -458,11 +456,11 @@ class ChopeNode : public rclcpp::Node request->ticket.ticket_id); if (waitpoint_result.has_value()) { - rmf_chope_msgs::msg::ReservationAllocation allocation; + rmf_reservation_msgs::msg::ReservationAllocation allocation; allocation.ticket = ticket_store_.get_existing_ticket( request->ticket.ticket_id); allocation.instruction_type = - rmf_chope_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY; + rmf_reservation_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY; allocation.satisfies_alternative = waitpoint_result.value(); allocation.resource = wait_points[waitpoint_result.value()].location; RCLCPP_INFO(this->get_logger(), "Allocating %s as waitpoint to %s", @@ -479,7 +477,7 @@ class ChopeNode : public rclcpp::Node } void release( - const rmf_chope_msgs::msg::ReleaseRequest::ConstSharedPtr& request) + const rmf_reservation_msgs::msg::ReleaseRequest::ConstSharedPtr& request) { RCLCPP_INFO( this->get_logger(), "Releasing ticket for %s", @@ -525,18 +523,18 @@ class ChopeNode : public rclcpp::Node if (!result.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the chope node's state and the", + this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the reservation node's state and the", ticket); return; } - rmf_chope_msgs::msg::ReservationAllocation allocation; + rmf_reservation_msgs::msg::ReservationAllocation allocation; allocation.satisfies_alternative = result.value(); allocation.resource = requests_[next_ticket.value()][result.value()].location; allocation.ticket = ticket_store_.get_existing_ticket(next_ticket.value()); allocation.instruction_type = - rmf_chope_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; RCLCPP_INFO( this->get_logger(), "Allocating %s to %s as a result of %s leaving", allocation.resource.c_str(), @@ -553,25 +551,25 @@ class ChopeNode : public rclcpp::Node void publish_free_spots() { - rmf_chope_msgs::msg::FreeParkingSpots spots; + rmf_reservation_msgs::msg::FreeParkingSpots spots; spots.spots = current_state_.free_locations(); free_spot_pub_->publish(spots); } - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr request_subscription_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr claim_subscription_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr release_subscription_; rclcpp::Subscription::SharedPtr graph_subscription_; - rclcpp::Publisher::SharedPtr ticket_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr ticket_pub_; + rclcpp::Publisher::SharedPtr allocation_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr free_spot_pub_; std::unordered_map> requests_; @@ -586,7 +584,7 @@ class ChopeNode : public rclcpp::Node int main(int argc, const char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index dce10bb04..25f86c9e6 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -28,7 +28,7 @@ find_package(yaml-cpp REQUIRED) find_package(nlohmann_json REQUIRED) find_package(ZLIB REQUIRED) find_package(LibUUID REQUIRED) -find_package(rmf_chope_msgs REQUIRED) +find_package(rmf_reservation_msgs REQUIRED) # NOTE(MXG): libproj-dev does not currently distribute its cmake config-files @@ -184,7 +184,7 @@ target_link_libraries(rmf_traffic_ros2 ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} ${rmf_building_map_msgs_LIBRARIES} - ${rmf_chope_msgs_LIBRARIES} + ${rmf_reservation_msgs_LIBRARIES} ${rclcpp_LIBRARIES} yaml-cpp ZLIB::ZLIB @@ -200,7 +200,7 @@ target_include_directories(rmf_traffic_ros2 ${rmf_traffic_msgs_INCLUDE_DIRS} ${rmf_site_map_msgs_INCLUDE_DIRS} ${rmf_building_map_msgs_INCLUDE_DIRS} - ${rmf_chope_msgs_INCLUDE_DIRS} + ${rmf_reservation_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index fc606bfbd..30ce29df8 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -16,7 +16,7 @@ proj rclcpp rmf_building_map_msgs - rmf_chope_msgs + rmf_reservation_msgs rmf_fleet_msgs rmf_site_map_msgs rmf_traffic_msgs From 0da49f377cbb98f8353b272c70d32efd2d39704d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 26 Sep 2024 20:36:40 +0800 Subject: [PATCH 097/119] Mostly style fixes address shared pointer capture Signed-off-by: Arjo Chakravarty --- .../events/EmergencyPullover.cpp | 4 +- .../events/EmergencyPullover.hpp | 4 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 14 +- .../rmf_fleet_adapter/events/GoToPlace.hpp | 4 +- ...=> internal_ReservationNodeNegotiator.hpp} | 160 ++++++++++-------- ...l_Utilities.cpp => internal_utilities.cpp} | 2 +- ...l_Utilities.hpp => internal_utilities.hpp} | 0 rmf_reservation_node/src/main.cpp | 1 - 8 files changed, 102 insertions(+), 87 deletions(-) rename rmf_fleet_adapter/src/rmf_fleet_adapter/events/{internal_ReservationNegotiator.hpp => internal_ReservationNodeNegotiator.hpp} (80%) rename rmf_fleet_adapter/src/rmf_fleet_adapter/events/{internal_Utilities.cpp => internal_utilities.cpp} (96%) rename rmf_fleet_adapter/src/rmf_fleet_adapter/events/{internal_Utilities.hpp => internal_utilities.hpp} (100%) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index ad1a4ca16..98f85c1a2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -24,7 +24,7 @@ #include #include "../project_itinerary.hpp" -#include "internal_Utilities.hpp" +#include "internal_utilities.hpp" namespace rmf_fleet_adapter { namespace events { @@ -175,7 +175,7 @@ auto EmergencyPullover::Active::make( RCLCPP_INFO(active->_context->node()->get_logger(), "Creating reservation negotiator"); - active->_reservation_client = reservation::reservationNodeNegotiator::make( + active->_reservation_client = reservation::ReservationNodeNegotiator::make( active->_context, potential_waitpoints, true, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp index 0ca7ace05..0a2dfcb1e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp @@ -23,7 +23,7 @@ #include "../services/FindEmergencyPullover.hpp" #include "../services/FindPath.hpp" -#include "internal_ReservationNegotiator.hpp" +#include "internal_ReservationNodeNegotiator.hpp" #include "ExecutePlan.hpp" @@ -129,7 +129,7 @@ class EmergencyPullover : public rmf_task_sequence::Event rclcpp::TimerBase::SharedPtr _find_path_timeout; std::optional _chosen_goal; - std::shared_ptr _reservation_client; + std::shared_ptr _reservation_client; bool _is_interrupted = false; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 74f3fd1e9..99ba4aa9a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -17,8 +17,8 @@ #include "GoToPlace.hpp" #include "../project_itinerary.hpp" -#include "internal_ReservationNegotiator.hpp" -#include "internal_Utilities.hpp" +#include "internal_ReservationNodeNegotiator.hpp" +#include "internal_utilities.hpp" #include "PerformAction.hpp" #include @@ -285,9 +285,7 @@ auto GoToPlace::Active::make( active->_is_final_destination = false; active->_chosen_goal = std::nullopt; - RCLCPP_INFO(active->_context->node()->get_logger(), - "Creating reservation negotiator"); - active->_reservation_client = std::move(reservation::reservationNodeNegotiator::make( + active->_reservation_client = std::move(reservation::ReservationNodeNegotiator::make( active->_context, active->_description.one_of(), active->_description.prefer_same_map(), @@ -340,6 +338,7 @@ void GoToPlace::Active::_on_reservation_node_allocate_waitpoint( RCLCPP_INFO(_context->node()->get_logger(), "Received waitpoint from reservation node"); _chosen_goal = goal; +_is_final_destination = false; _find_plan(); } @@ -478,11 +477,10 @@ std::optional GoToPlace::Active::_choose_goal( if (_context->_parking_spot_manager_enabled()) { RCLCPP_INFO(_context->node()->get_logger(), - "Requesting reservation Node For Time To Progress"); + "Waiting for next location from reservation node."); } else { - RCLCPP_INFO(_context->node()->get_logger(), "Skipping reservation node."); auto lowest_cost = std::numeric_limits::infinity(); std::optional selected_idx; for (std::size_t i = 0; i < _description.one_of().size(); ++i) @@ -579,7 +577,7 @@ void GoToPlace::Active::_find_plan() { RCLCPP_ERROR( _context->node()->get_logger(), - "Robot [%s] is lost retrying.", + "Robot [%s] is lost while trying to find a plan.", _context->requester_id().c_str()); _schedule_retry(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 0e63b485e..3b64592e4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -24,7 +24,7 @@ #include "../services/FindPath.hpp" #include "ExecutePlan.hpp" -#include "internal_ReservationNegotiator.hpp" +#include "internal_ReservationNodeNegotiator.hpp" #include #include @@ -154,7 +154,7 @@ class GoToPlace : public rmf_task_sequence::Event rmf_rxcpp::subscription_guard _replan_request_subscription; rmf_rxcpp::subscription_guard _graph_change_subscription; - std::shared_ptr _reservation_client; + std::shared_ptr _reservation_client; bool _is_interrupted = false; bool _is_final_destination = true; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp similarity index 80% rename from rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNegotiator.hpp rename to rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index f9d8c3758..1f2a540cf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -15,8 +15,8 @@ * */ -#ifndef SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_reservation_NEGOTIATOR_HPP -#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_reservation_NEGOTIATOR_HPP +#ifndef SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_RESERVATION_NEGOTIATOR_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_RESERVATION_NEGOTIATOR_HPP #include "../agv/RobotContext.hpp" @@ -27,11 +27,11 @@ namespace reservation { /// This class implements the protocol for negotiating a spot with the "reservation" /// node. The reservation node maintains a list of spots which are free. -class reservationNodeNegotiator : - public std::enable_shared_from_this +class ReservationNodeNegotiator : + public std::enable_shared_from_this { public: - reservationNodeNegotiator( + ReservationNodeNegotiator( std::shared_ptr context, const std::vector goals, const bool same_map, @@ -44,12 +44,54 @@ class reservationNodeNegotiator : _selected_final_destination_cb = std::move(selected_final_destination_cb); _selected_waitpoint_cb = std::move(selected_waitpoint_cb); _reservation_id = _context->last_reservation_request_id(); - _reservation_ticket = - _context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( - _context->worker())) - .subscribe([self = this](const std::shared_ptr + } + + void force_release() + { + std::unordered_set released_ticket_ids; + while (auto allocation = _context->_release_resource()) + { + if (released_ticket_ids.count(allocation->ticket.ticket_id) != 0) + { + continue; + } + released_ticket_ids.insert(allocation->ticket.ticket_id); + rmf_reservation_msgs::msg::ReleaseRequest msg; + msg.ticket = allocation->ticket; + _context->node()->release_location()->publish(msg); + RCLCPP_INFO( + _context->node()->get_logger(), + "reservation: Releasing waypoint for ticket %lu", + msg.ticket.ticket_id + ); + } + } + + static std::shared_ptr make( + std::shared_ptr context, + const std::vector goals, + const bool same_map, + const std::function + selected_final_destination_cb, + const std::function selected_waitpoint_cb) + { + RCLCPP_INFO(context->node()->get_logger(), + "Constructing reservation negotiator"); + auto negotiator = std::make_shared(context, + goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); + + negotiator->_reservation_ticket = + context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( + context->worker())) + .subscribe([ptr = negotiator->weak_from_this()]( + const std::shared_ptr & msg) { + auto self = ptr.lock(); + if(!self) + { + return; + } RCLCPP_INFO( self->_context->node()->get_logger(), @@ -70,7 +112,7 @@ class reservationNodeNegotiator : { RCLCPP_ERROR( self->_context->node()->get_logger(), - "reservation: Got no waitpoints"); + "Reservation: Got no waitpoints"); return; } @@ -85,18 +127,23 @@ class reservationNodeNegotiator : } self->_context->node()->claim_location_ticket()->publish( claim_request); - RCLCPP_ERROR( + RCLCPP_DEBUG( self->_context->node()->get_logger(), - "reservation: Claim issued"); + "Reservation: Claim issued by %s", self->_context->name().c_str()); }); - _reservation_allocation = - _context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( - _context->worker())) - .subscribe([self = this](const std::shared_ptr + negotiator->_reservation_allocation = + context->node()->allocated_claims_obs().observe_on(rxcpp::identity_same_worker( + context->worker())) + .subscribe([ptr = negotiator->weak_from_this()](const std::shared_ptr & msg) { + auto self = ptr.lock(); + if(!self) + { + return; + } if (!self->_ticket.has_value()) { return; @@ -120,7 +167,7 @@ class reservationNodeNegotiator : { RCLCPP_INFO( self->_context->node()->get_logger(), - "reservation: Robot %s is going to final destination %lu", + "Reservation: Robot %s is going to final destination %lu", self->_context->name().c_str(), self->_goals[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); @@ -139,7 +186,7 @@ class reservationNodeNegotiator : satisfies_alternative]); RCLCPP_INFO( self->_context->node()->get_logger(), - "reservationvationvationvation: Robot %s is being asked to proceed to a waitpoint %lu", + "Reservation: Robot %s is being asked to proceed to a waitpoint %lu", self->_context->name().c_str(), self->_waitpoints[self->_final_allocated_destination.value()-> satisfies_alternative].waypoint()); @@ -147,88 +194,59 @@ class reservationNodeNegotiator : }); - auto current_location = _context->location(); + auto current_location = context->location(); if (current_location.size() == 0) { using namespace std::literals::chrono_literals; - _retry_timer = _context->node()->create_wall_timer( - 500ms, [this, same_map]() + negotiator->_retry_timer = context->node()->create_wall_timer( + 500ms, [self = negotiator->weak_from_this(), same_map]() { - auto current_location = _context->location(); + auto negotiator = self.lock(); + if(!negotiator) + { + return; + } + + auto current_location = negotiator->_context->location(); if (current_location.size() != 0) { - _retry_timer->cancel(); + negotiator->_retry_timer->cancel(); } else { return; } - for (std::size_t i = 0; i < _goals.size(); ++i) + for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) { - if (_goals[i].waypoint() == current_location[0].waypoint()) + if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) { - RCLCPP_ERROR(_context->node()->get_logger(), + RCLCPP_ERROR(negotiator->_context->node()->get_logger(), "Already at goal no need to engage reservation system\n"); - _selected_final_destination_cb(_goals[i].waypoint()); + negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); return; } } - RCLCPP_INFO(_context->node()->get_logger(), + RCLCPP_INFO(negotiator->_context->node()->get_logger(), "Sending reservation request"); - make_request(same_map); + negotiator->make_request(same_map); } ); - return; + return negotiator; } - for (std::size_t i = 0; i < _goals.size(); ++i) + for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) { - if (_goals[i].waypoint() == current_location[0].waypoint()) + if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) { - RCLCPP_ERROR(_context->node()->get_logger(), + RCLCPP_ERROR(context->node()->get_logger(), "Already at goal no need to engage reservation system\n"); - _selected_final_destination_cb(_goals[i]); - return; + negotiator->_selected_final_destination_cb(negotiator->_goals[i]); + return negotiator; } } RCLCPP_INFO(context->node()->get_logger(), "Sending reservation request"); - make_request(same_map); - } - - void force_release() - { - std::unordered_set released_ticket_ids; - while (auto allocation = _context->_release_resource()) - { - if (released_ticket_ids.count(allocation->ticket.ticket_id) != 0) - { - continue; - } - released_ticket_ids.insert(allocation->ticket.ticket_id); - rmf_reservation_msgs::msg::ReleaseRequest msg; - msg.ticket = allocation->ticket; - _context->node()->release_location()->publish(msg); - RCLCPP_INFO( - _context->node()->get_logger(), - "reservation: Releasing waypoint for ticket %lu", - msg.ticket.ticket_id - ); - } - } - - static std::shared_ptr make( - std::shared_ptr context, - const std::vector goals, - const bool same_map, - const std::function - selected_final_destination_cb, - const std::function selected_waitpoint_cb) - { - RCLCPP_INFO(context->node()->get_logger(), - "Constructing reservation negotiator"); - auto negotiator = std::make_shared(context, - goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); + negotiator->make_request(same_map); return negotiator; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp similarity index 96% rename from rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp rename to rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp index 5924225dc..02a82f809 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp @@ -1,4 +1,4 @@ -#include "internal_Utilities.hpp" +#include "internal_utilities.hpp" namespace rmf_fleet_adapter { namespace events { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.hpp similarity index 100% rename from rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_Utilities.hpp rename to rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.hpp diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index 8ce6c29bd..056a0348c 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -36,7 +36,6 @@ struct std::hash /// Ticket generation class for book keeping purposes. Will eventually overflow. -/// Ticket id 0 does not exist and is useful for making emergency claims. /// Ticket ids are mapped across multiple fleets. class TicketStore { From 646de634534e66323a02a2587fe569302bc6b71b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 28 Sep 2024 15:18:46 +0800 Subject: [PATCH 098/119] More style fixes Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/events/GoToPlace.cpp | 2 +- .../internal_ReservationNodeNegotiator.hpp | 10 +++--- rmf_reservation_node/src/main.cpp | 32 ++++++++++--------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 99ba4aa9a..58bd04b53 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -338,7 +338,7 @@ void GoToPlace::Active::_on_reservation_node_allocate_waitpoint( RCLCPP_INFO(_context->node()->get_logger(), "Received waitpoint from reservation node"); _chosen_goal = goal; -_is_final_destination = false; + _is_final_destination = false; _find_plan(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index 1f2a540cf..ead8c7af0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -75,8 +75,6 @@ class ReservationNodeNegotiator : selected_final_destination_cb, const std::function selected_waitpoint_cb) { - RCLCPP_INFO(context->node()->get_logger(), - "Constructing reservation negotiator"); auto negotiator = std::make_shared(context, goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); @@ -95,7 +93,7 @@ class ReservationNodeNegotiator : RCLCPP_INFO( self->_context->node()->get_logger(), - "reservation: Got ticket issueing claim"); + "Reservations: Got ticket issueing claim"); if (msg->header.request_id != self->_reservation_id || msg->header.robot_name != self->_context->name() @@ -112,7 +110,7 @@ class ReservationNodeNegotiator : { RCLCPP_ERROR( self->_context->node()->get_logger(), - "Reservation: Got no waitpoints"); + "Reservations: Got no waitpoints"); return; } @@ -129,7 +127,7 @@ class ReservationNodeNegotiator : claim_request); RCLCPP_DEBUG( self->_context->node()->get_logger(), - "Reservation: Claim issued by %s", self->_context->name().c_str()); + "Reservations: Claim issued by %s", self->_context->name().c_str()); }); @@ -269,7 +267,7 @@ class ReservationNodeNegotiator : const auto& graph = _context->navigation_graph(); if (current_location.size() == 0) { - //unable to get location. We should return some form of error stste. + //unable to get location. We should return some form of error state. RCLCPP_ERROR( _context->node()->get_logger(), "reservation: Robot [%s] can't get location", diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index 056a0348c..e0baff340 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -46,10 +46,10 @@ class TicketStore { rmf_reservation_msgs::msg::Ticket ticket; ticket.header = request_header; - ticket.ticket_id = _last_issued_ticket_id; + ticket.ticket_id = _next_ticket_id; - _ticket_to_header.emplace(_last_issued_ticket_id, request_header); - _last_issued_ticket_id++; + _ticket_to_header.emplace(_next_ticket_id, request_header); + _next_ticket_id++; return ticket; } @@ -74,7 +74,7 @@ class TicketStore std::unordered_map _ticket_to_header; - std::size_t _last_issued_ticket_id = 1; + std::size_t _next_ticket_id = 1; }; struct LocationState @@ -303,10 +303,10 @@ class ServiceQueueManager using namespace std::chrono_literals; -class reservationNode : public rclcpp::Node +class ReservationNode : public rclcpp::Node { public: - reservationNode() + ReservationNode() : Node("rmf_reservation_node") { @@ -318,21 +318,21 @@ class reservationNode : public rclcpp::Node request_subscription_ = this->create_subscription( ReservationRequestTopicName, qos, - std::bind(&reservationNode::on_request, this, + std::bind(&ReservationNode::on_request, this, std::placeholders::_1)); claim_subscription_ = this->create_subscription( ReservationClaimTopicName, qos, - std::bind(&reservationNode::claim_request, this, + std::bind(&ReservationNode::claim_request, this, std::placeholders::_1)); release_subscription_ = this->create_subscription( ReservationReleaseTopicName, qos, - std::bind(&reservationNode::release, this, std::placeholders::_1)); + std::bind(&ReservationNode::release, this, std::placeholders::_1)); graph_subscription_ = this->create_subscription( "/nav_graphs", qos, - std::bind(&reservationNode::received_graph, this, + std::bind(&ReservationNode::received_graph, this, std::placeholders::_1)); ticket_pub_ = this->create_publisher( @@ -346,7 +346,7 @@ class reservationNode : public rclcpp::Node timer_ = this->create_wall_timer(500ms, - std::bind(&reservationNode::publish_free_spots, this)); + std::bind(&ReservationNode::publish_free_spots, this)); } private: @@ -513,7 +513,7 @@ class reservationNode : public rclcpp::Node current_state_.allocate_lowest_cost_free_spot(requests_[next_ticket. value()], next_ticket.value()); - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "Found next item %lu on queue %s", next_ticket.value(), released_location.value().c_str()); @@ -522,7 +522,9 @@ class reservationNode : public rclcpp::Node if (!result.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Tried to service %lu. Apparently there was some inconsitency between the reservation node's state and the", + this->get_logger(), + "Tried to service %lu. Apparently there was some inconsistency" + " between the reservation node's state and the fleetadapter.", ticket); return; } @@ -541,7 +543,7 @@ class reservationNode : public rclcpp::Node ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); } - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "Queue is now empty %s", released_location->c_str()); return; @@ -583,7 +585,7 @@ class reservationNode : public rclcpp::Node int main(int argc, const char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file From a00d34ccc80595fd92f12d431efb9aab35a1d436 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Sep 2024 15:24:25 +0800 Subject: [PATCH 099/119] More style fixes Signed-off-by: Arjo Chakravarty --- .../events/internal_ReservationNodeNegotiator.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index ead8c7af0..5e4a4db5e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -34,7 +34,6 @@ class ReservationNodeNegotiator : ReservationNodeNegotiator( std::shared_ptr context, const std::vector goals, - const bool same_map, const std::function selected_final_destination_cb, const std::function selected_waitpoint_cb) @@ -76,7 +75,7 @@ class ReservationNodeNegotiator : const std::function selected_waitpoint_cb) { auto negotiator = std::make_shared(context, - goals, same_map, selected_final_destination_cb, selected_waitpoint_cb); + goals, selected_final_destination_cb, selected_waitpoint_cb); negotiator->_reservation_ticket = context->node()->location_ticket_obs().observe_on(rxcpp::identity_same_worker( From b57cb3e2ee677d58cbcdde2b908cfdab3a04286c Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Sep 2024 15:25:39 +0800 Subject: [PATCH 100/119] Rework how configurations are loaded so as not to break abi Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 17 +++++---- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 4 +-- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 35 ++++++++----------- .../agv/internal_EasyFullControl.hpp | 8 ++--- 4 files changed, 29 insertions(+), 35 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index 86a13e93b..b3cd6e2c4 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -265,7 +265,6 @@ class EasyFullControl::RobotConfiguration RobotConfiguration( std::vector compatible_chargers, std::optional responsive_wait = std::nullopt, - bool use_parking_spot_system = false, std::optional max_merge_waypoint_distance = 1e-3, std::optional max_merge_lane_distance = 0.3, std::optional min_lane_length = 1e-8); @@ -285,8 +284,7 @@ class EasyFullControl::RobotConfiguration /// be used. std::optional responsive_wait() const; - /// Check if the robot should use the parking spot system. - bool use_parking_spot_system() const; + /// Toggle responsive wait on (true), off (false), or use fleet default /// (std::nullopt). @@ -566,9 +564,6 @@ class EasyFullControl::FleetConfiguration /// /// \param[in] default_min_lane_length /// The minimum length that a lane should have. - /// - /// \param[in] use_parking_reservation - /// Whether to use parking_reservations. FleetConfiguration( const std::string& fleet_name, std::optional> @@ -595,8 +590,8 @@ class EasyFullControl::FleetConfiguration bool default_responsive_wait = false, double default_max_merge_waypoint_distance = 1e-3, double default_max_merge_lane_distance = 0.3, - double min_lane_length = 1e-8, - bool use_parking_reservation = false + double min_lane_length = 1e-8 + ); /// Create a FleetConfiguration object using a set of configuration parameters @@ -779,7 +774,11 @@ class EasyFullControl::FleetConfiguration bool default_responsive_wait() const; /// Should robots use the parking reservation system. - bool use_parking_reservation_system() const; + bool using_parking_reservation_system() const; + + /// Set whether this fleet uses the parking reservation system. + void use_parking_reservation_system( + const bool use); /// Set whether robots in this fleet should have responsive wait enabled by /// default. diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index 039e92493..a7150f370 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -381,10 +381,10 @@ std::shared_ptr Adapter::add_easy_fleet( tf_dict, config.strict_lanes(), config.default_responsive_wait(), - config.use_parking_reservation_system(), config.default_max_merge_waypoint_distance(), config.default_max_merge_lane_distance(), - config.default_min_lane_length()); + config.default_min_lane_length(), + config.using_parking_reservation_system()); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index e19db12a0..addba8005 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -122,14 +122,12 @@ class EasyFullControl::RobotConfiguration::Implementation std::optional max_merge_waypoint_distance; std::optional max_merge_lane_distance; std::optional min_lane_length; - bool use_parking_spot; }; //============================================================================== EasyFullControl::RobotConfiguration::RobotConfiguration( std::vector compatible_chargers, std::optional responsive_wait, - bool use_parking_spot_system, std::optional max_merge_waypoint_distance, std::optional max_merge_lane_distance, std::optional min_lane_length) @@ -139,7 +137,6 @@ EasyFullControl::RobotConfiguration::RobotConfiguration( max_merge_waypoint_distance, max_merge_lane_distance, min_lane_length, - use_parking_spot_system })) { // Do nothing @@ -165,11 +162,6 @@ std::optional EasyFullControl::RobotConfiguration::responsive_wait() const return _pimpl->responsive_wait; } -//============================================================================== -bool EasyFullControl::RobotConfiguration::use_parking_spot_system() const -{ - return _pimpl->use_parking_spot; -} //============================================================================== void EasyFullControl::RobotConfiguration::set_responsive_wait( @@ -1883,8 +1875,7 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( bool default_responsive_wait, double default_max_merge_waypoint_distance, double default_max_merge_lane_distance, - double default_min_lane_length, - bool use_parking_reservation) + double default_min_lane_length) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(fleet_name), @@ -1913,7 +1904,7 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(default_min_lane_length), {}, {}, - std::move(use_parking_reservation) + false })) { // Do nothing @@ -2464,7 +2455,6 @@ EasyFullControl::FleetConfiguration::from_config_files( auto config = RobotConfiguration( std::move(chargers), responsive_wait, - use_simple_parking_reservation_system, max_merge_waypoint_distance, max_merge_lane_distance, min_lane_length); @@ -2619,10 +2609,10 @@ EasyFullControl::FleetConfiguration::from_config_files( default_responsive_wait, default_max_merge_waypoint_distance, default_max_merge_lane_distance, - default_min_lane_length, - use_simple_parking_reservation_system); + default_min_lane_length); config.change_lift_emergency_levels() = lift_emergency_levels; config.set_retreat_to_charger_interval(retreat_to_charger_interval); + config.use_parking_reservation_system(use_simple_parking_reservation_system); config.change_strict_lanes() = std::move(strict_lanes); return config; } @@ -2937,12 +2927,19 @@ bool EasyFullControl::FleetConfiguration::default_responsive_wait() const } //============================================================================== -bool EasyFullControl::FleetConfiguration::use_parking_reservation_system() +bool EasyFullControl::FleetConfiguration::using_parking_reservation_system() const { return _pimpl->use_parking_reservation; } +//============================================================================== +void EasyFullControl::FleetConfiguration::use_parking_reservation_system( + const bool use) +{ + _pimpl->use_parking_reservation = use; +} + //============================================================================== void EasyFullControl::FleetConfiguration::set_default_responsive_wait( bool enable) @@ -3231,8 +3228,6 @@ auto EasyFullControl::add_robot( enable_responsive_wait = *configuration.responsive_wait(); } - bool enable_parking_spot = configuration.use_parking_spot_system(); - _pimpl->fleet_handle->add_robot( insertion.first->second, robot_name, @@ -3249,7 +3244,7 @@ auto EasyFullControl::add_robot( localization = std::move(localization), nav_params = robot_nav_params, enable_responsive_wait, - enable_parking_spot + use_parking_reservation = _pimpl->use_parking_reservation ](const RobotUpdateHandlePtr& updater) { auto context = RobotUpdateHandle::Implementation::get(*updater) @@ -3269,7 +3264,7 @@ auto EasyFullControl::add_robot( context, nav_params, enable_responsive_wait, - enable_parking_spot + use_parking_reservation ](const auto&) { cmd_handle->w_context = context; @@ -3285,7 +3280,7 @@ auto EasyFullControl::add_robot( } handle->enable_responsive_wait(enable_responsive_wait); - context->_set_parking_spot_manager(enable_parking_spot); + context->_set_parking_spot_manager(use_parking_reservation); RCLCPP_INFO( node->get_logger(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp index 74b52372c..4cc50ab29 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp @@ -36,7 +36,7 @@ class EasyFullControl::Implementation std::unordered_map cmd_handles; NavParams nav_params; bool default_responsive_wait; - bool use_parking_reservation_node; + bool use_parking_reservation; static std::shared_ptr make( std::shared_ptr fleet_handle, @@ -44,10 +44,10 @@ class EasyFullControl::Implementation std::shared_ptr transforms_to_robot_coords, std::unordered_set strict_lanes, bool default_responsive_wait, - bool use_parking_reservation_node, double default_max_merge_waypoint_distance, double default_max_merge_lane_distance, - double default_min_lane_length) + double default_min_lane_length, + bool use_parking_reservation) { auto handle = std::shared_ptr(new EasyFullControl); handle->_pimpl = rmf_utils::make_unique_impl( @@ -63,7 +63,7 @@ class EasyFullControl::Implementation default_min_lane_length, }, default_responsive_wait, - use_parking_reservation_node + use_parking_reservation }); return handle; } From f2fced6c90a180d8c39886c4eb8c6b1df54e6869 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Sep 2024 15:40:52 +0800 Subject: [PATCH 101/119] More cleanups Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 6 ------ .../src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 9 ++++----- .../src/rmf_fleet_adapter/agv/Node.cpp | 5 ----- .../src/rmf_fleet_adapter/agv/Node.hpp | 13 ++++--------- 4 files changed, 8 insertions(+), 25 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index b3cd6e2c4..b54c94914 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -255,9 +255,6 @@ class EasyFullControl::RobotConfiguration /// \param[in] responsive_wait /// Should this robot use the responsive wait behavior? true / false / fleet default. /// - /// \param[in] use_parking_spot - /// Use parking spot system. - /// /// \warning This must contain a single string value until a later release of /// RMF. We are using a vector for forward API compatibility. For now, make /// sure each robot has only one unique compatible charger to avoid charging @@ -284,8 +281,6 @@ class EasyFullControl::RobotConfiguration /// be used. std::optional responsive_wait() const; - - /// Toggle responsive wait on (true), off (false), or use fleet default /// (std::nullopt). void set_responsive_wait(std::optional enable); @@ -591,7 +586,6 @@ class EasyFullControl::FleetConfiguration double default_max_merge_waypoint_distance = 1e-3, double default_max_merge_lane_distance = 0.3, double min_lane_length = 1e-8 - ); /// Create a FleetConfiguration object using a set of configuration parameters diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index addba8005..7edf3c0c3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -136,7 +136,7 @@ EasyFullControl::RobotConfiguration::RobotConfiguration( responsive_wait, max_merge_waypoint_distance, max_merge_lane_distance, - min_lane_length, + min_lane_length })) { // Do nothing @@ -162,7 +162,6 @@ std::optional EasyFullControl::RobotConfiguration::responsive_wait() const return _pimpl->responsive_wait; } - //============================================================================== void EasyFullControl::RobotConfiguration::set_responsive_wait( std::optional enable) @@ -1904,7 +1903,7 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(default_min_lane_length), {}, {}, - false + false // Parking reservation system })) { // Do nothing @@ -2928,14 +2927,14 @@ bool EasyFullControl::FleetConfiguration::default_responsive_wait() const //============================================================================== bool EasyFullControl::FleetConfiguration::using_parking_reservation_system() -const + const { return _pimpl->use_parking_reservation; } //============================================================================== void EasyFullControl::FleetConfiguration::use_parking_reservation_system( - const bool use) + const bool use) { _pimpl->use_parking_reservation = use; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 96dfc1541..15faa2c83 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -310,10 +310,5 @@ auto Node::release_location() const -> const ReservationReleasePub& return _reservation_release_pub; } -//============================================================================== -auto Node::freespots_obs() const -> const ReservationFreeSpotObs& -{ - return _reservation_free_spot_obs->observe(); -} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 348b7d506..1af3d8333 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -33,12 +33,12 @@ #include #include #include -#include #include -#include -#include -#include +#include #include +#include +#include +#include #include @@ -166,10 +166,6 @@ class Node : public rmf_rxcpp::Transport rclcpp::Publisher::SharedPtr; const ReservationReleasePub& release_location() const; - using ReservationFreeSpotStatus = rmf_reservation_msgs::msg::FreeParkingSpots; - using ReservationFreeSpotObs = - rxcpp::observable; - const ReservationFreeSpotObs& freespots_obs() const; template rclcpp::TimerBase::SharedPtr try_create_wall_timer( @@ -236,7 +232,6 @@ class Node : public rmf_rxcpp::Transport ReservationClaimPub _reservation_claim_pub; Bridge _reservation_alloc_obs; ReservationReleasePub _reservation_release_pub; - Bridge _reservation_free_spot_obs; }; } // namespace agv From 3e5ccc2024cb2ff753090deaed2be436867f9c62 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Sep 2024 15:48:37 +0800 Subject: [PATCH 102/119] More cleanups Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/events/EmergencyPullover.cpp | 8 ++++---- .../src/rmf_fleet_adapter/events/ExecutePlan.cpp | 2 +- .../src/rmf_fleet_adapter/events/internal_utilities.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 98f85c1a2..9ca6a41af 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -359,19 +359,19 @@ void EmergencyPullover::Active::_find_plan() { _find_path_service = std::make_shared( - _context->planner(), _context->location(), *_chosen_goal, + _context->planner(), _context->location(), _chosen_goal.value(), _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile(), std::chrono::seconds(5)); const auto start_name = wp_name(*_context); - const auto goal_name = wp_name(*_context, *_chosen_goal); + const auto goal_name = wp_name(*_context, _chosen_goal.value()); _plan_subscription = rmf_rxcpp::make_job( _find_path_service) .observe_on(rxcpp::identity_same_worker(_context->worker())) .subscribe( - [w = weak_from_this(), start_name, goal_name, goal = *_chosen_goal]( + [w = weak_from_this(), start_name, goal_name, goal = _chosen_goal.value()]( const services::FindPath::Result& result) { const auto self = w.lock(); @@ -406,7 +406,7 @@ void EmergencyPullover::Active::_find_plan() std::vector no_future_projections; auto full_itinerary = project_itinerary( - *result, no_future_projections, + *result, {}, *self->_context->planner()); self->_execute_plan( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 1be96dd78..2e7c99a52 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -519,7 +519,7 @@ std::optional search_for_lift_group( auto final_lift_destination = agv::RobotUpdateHandle::LiftDestination::Implementation::make( - lift_name, lift_end->destination()); + lift_name, lift_end->destination()); auto category = "Take [lift:" + lift_name + "] to [floor:" + lift_end->destination() + "]"; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp index 02a82f809..6929a6b86 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_utilities.cpp @@ -35,4 +35,4 @@ std::string wp_name(const agv::RobotContext& context) } } -} \ No newline at end of file +} From fc345f9edae05a1674cf5c9cd7e709474ac9a80e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 30 Sep 2024 15:59:25 +0800 Subject: [PATCH 103/119] Copyright headers Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/ReservationManager.hpp | 2 +- rmf_reservation_node/src/main.cpp | 16 ++++++++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index b4f0e3f07..bf378e7cc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2024 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. diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index e0baff340..14823527a 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -1,3 +1,19 @@ +/* + * Copyright (C) 2024 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 "rclcpp/rclcpp.hpp" #include From 19f1eb2c089869a01733c6e0e9a4589964c4ec32 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 1 Oct 2024 12:31:26 +0800 Subject: [PATCH 104/119] Move make_request to worker thread lso make sure we only use the reservation system if we need to move. Signed-off-by: Arjo Chakravarty --- .../internal_ReservationNodeNegotiator.hpp | 97 ++++++++++--------- rmf_reservation_node/src/main.cpp | 25 ++--- 2 files changed, 67 insertions(+), 55 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index 5e4a4db5e..c9c3e4435 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -190,60 +190,69 @@ class ReservationNodeNegotiator : } }); - - auto current_location = context->location(); - if (current_location.size() == 0) + context->worker().schedule( + [ptr = negotiator->weak_from_this(), w = context->weak_from_this(), same_map]( + const auto&) { - using namespace std::literals::chrono_literals; - negotiator->_retry_timer = context->node()->create_wall_timer( - 500ms, [self = negotiator->weak_from_this(), same_map]() - { - auto negotiator = self.lock(); - if(!negotiator) - { - return; - } + auto context = w.lock(); + if (!context) + return; - auto current_location = negotiator->_context->location(); - if (current_location.size() != 0) - { - negotiator->_retry_timer->cancel(); - } - else - { - return; - } - for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) + auto negotiator = ptr.lock(); + + auto current_location = context->location(); + if (current_location.size() == 0) + { + using namespace std::literals::chrono_literals; + negotiator->_retry_timer = context->node()->create_wall_timer( + 500ms, [self = negotiator->weak_from_this(), same_map]() { - if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) + auto negotiator = self.lock(); + if(!negotiator) { - RCLCPP_ERROR(negotiator->_context->node()->get_logger(), - "Already at goal no need to engage reservation system\n"); - negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); return; } + + auto current_location = negotiator->_context->location(); + if (current_location.size() != 0) + { + negotiator->_retry_timer->cancel(); + } + else + { + return; + } + for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) + { + if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) + { + RCLCPP_ERROR(negotiator->_context->node()->get_logger(), + "Already at goal no need to engage reservation system\n"); + negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); + return; + } + } + RCLCPP_INFO(negotiator->_context->node()->get_logger(), + "Sending reservation request"); + negotiator->make_request(same_map); } - RCLCPP_INFO(negotiator->_context->node()->get_logger(), - "Sending reservation request"); - negotiator->make_request(same_map); - } - ); - return negotiator; - } + ); + } - for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) - { - if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) + for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) { - RCLCPP_ERROR(context->node()->get_logger(), - "Already at goal no need to engage reservation system\n"); - negotiator->_selected_final_destination_cb(negotiator->_goals[i]); - return negotiator; + if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) + { + RCLCPP_ERROR(context->node()->get_logger(), + "Already at goal no need to engage reservation system\n"); + negotiator->_selected_final_destination_cb(negotiator->_goals[i]); + return; + } } - } - RCLCPP_INFO(context->node()->get_logger(), - "Sending reservation request"); - negotiator->make_request(same_map); + RCLCPP_INFO(context->node()->get_logger(), + "Sending reservation request"); + negotiator->make_request(same_map); + }); return negotiator; } diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index 14823527a..553847459 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -432,7 +432,7 @@ class ReservationNode : public rclcpp::Node allocation.resource = requests_[request->ticket.ticket_id][result.value()].location; - RCLCPP_INFO(this->get_logger(), "Allocating %s to %s", + RCLCPP_DEBUG(this->get_logger(), "Allocating %s to %s", allocation.resource.c_str(), ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); allocation_pub_->publish(allocation); @@ -440,15 +440,17 @@ class ReservationNode : public rclcpp::Node } // If we can't proceed immediately add the ticket to a queue. - RCLCPP_INFO( - this->get_logger(), "Could not immediately service %lu, enqueing. Locations in ticket were:", - request->ticket.ticket_id); + std::stringstream ss; + ss << "Could not immediately service %lu, enqueing. Locations in ticket were:" << + request->ticket.ticket_id << std::endl; for (std::size_t i = 0; i < requests_[request->ticket.ticket_id].size(); i++) { - RCLCPP_INFO(this->get_logger(), "\t- %s", - requests_[request->ticket.ticket_id][i].location.c_str()); + ss << "\t- %s" << requests_[request->ticket.ticket_id][i].location.c_str() << std::endl; } + + RCLCPP_INFO( + this->get_logger(), "%s", ss.str().c_str()); queue_manager_.add_to_queue(request->ticket.ticket_id, location_names); // Allocate a waitpoint by preference as given by Fleet Adapter @@ -494,7 +496,7 @@ class ReservationNode : public rclcpp::Node void release( const rmf_reservation_msgs::msg::ReleaseRequest::ConstSharedPtr& request) { - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "Releasing ticket for %s", ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); auto ticket = request->ticket.ticket_id; @@ -502,11 +504,11 @@ class ReservationNode : public rclcpp::Node if (!released_location.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Could not find ticket %lu", + this->get_logger(), "Could not find ticket %lu. Something is wrong.", request->ticket.ticket_id); return; } - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "Released ticket %lu, location %s is now free", request->ticket.ticket_id, released_location->c_str()); @@ -520,7 +522,8 @@ class ReservationNode : public rclcpp::Node if (!released_location.has_value()) { RCLCPP_ERROR( - this->get_logger(), "Could not find ticket %lu while traversing wait graph", + this->get_logger(), + "Could not find ticket %lu while traversing wait graph. Something has gone wrong.", next_ticket.value()); return; } @@ -552,7 +555,7 @@ class ReservationNode : public rclcpp::Node ticket_store_.get_existing_ticket(next_ticket.value()); allocation.instruction_type = rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - RCLCPP_INFO( + RCLCPP_DEBUG( this->get_logger(), "Allocating %s to %s as a result of %s leaving", allocation.resource.c_str(), ticket_store_.debug_ticket(next_ticket.value()).c_str(), From 66ea3ba758c7530a31a8f1afd5d2574b2f9be218 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 1 Oct 2024 15:54:35 +0800 Subject: [PATCH 105/119] Style Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 17b417b37..7746b4410 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -15,11 +15,9 @@ * */ -#include "RobotContext.hpp" #include "internal_RobotUpdateHandle.hpp" #include "../TaskManager.hpp" -#include #include #include @@ -1843,8 +1841,7 @@ std::vector RobotContext::_find_and_sort_parking_spots( const bool same_floor) const { - std::vector - final_result; + std::vector final_result; // Retrieve nav graph const auto& graph = navigation_graph(); From 5e74ac288b3ffeb4bf364f6f007527c81b93870e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 1 Oct 2024 15:58:10 +0800 Subject: [PATCH 106/119] Clean up readme Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/events/EmergencyPullover.cpp | 1 - rmf_reservation_node/README.md | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 9ca6a41af..c53f2811b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -29,7 +29,6 @@ namespace rmf_fleet_adapter { namespace events { - //============================================================================== class EmergencyPulloverDescription : public rmf_task_sequence::events::Placeholder::Description diff --git a/rmf_reservation_node/README.md b/rmf_reservation_node/README.md index 95d910185..6689e67d6 100644 --- a/rmf_reservation_node/README.md +++ b/rmf_reservation_node/README.md @@ -7,7 +7,6 @@ Before a robot goes to its next destination the fleet adapter asks the reservati ``` We recommend disabling `responsive_wait` when you do so. An example is available in `rmf_demos`. -Some immediate limitations of the reservation node is that it is limited to scenarios with a single navigation graph. Overcoming this limitation should not be too much work now that the basic infrastructure is in place. ## Expected Behaviour @@ -39,11 +38,12 @@ ros2 run rmf_demos_tasks dispatch_go_to_place -p lounge -F tinyRobot -R tinyRobo ``` `tinyRobot1` and `tinyRobot2` should now proceed to swap places. -If you have the same number or more waitpoints than robots you should never have a deadlock. +If there are at least as many parking spots as robots on any given floor, then you should never have a deadlock. ## Protocol Used Behind the Scenes -The reservation node has no information about the state of the graph, it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace` event. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleet adapter submits a ReservationRequest message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The reservation node will issue a ticket for said request. When ready to proceed, send a claim message with the ticket and list of potential waiting points ordered by distance. The reservation node will then try to award the lowest cost target location. If it can't it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. +The reservation node has no information about the state of the graph, it simply maintains a list of available parking spots internally. The fleet adapter does most of the heavy lifting in the `GoToPlace` event. When a `GoToPlace` event is started we first check if the robot is already at one of the target locations. The fleet adapter submits a ReservationRequest message with the desired end parameters. The parameters are listed in terms of cost (where the cost function is distance). The reservation node will issue a ticket for said request. When ready to proceed, send a claim message with the ticket and list of potential waiting points ordered by distance. The reservation node will then try to award the lowest cost target location. If the destination is not available yet then it will award the lowest cost waiting point to the robot. The fleet adapter will release the previous location as it starts to move to its next location. + ## Known Issues From f9d7195d34d09c023416ea54bb61cf5d354a79ce Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 2 Oct 2024 10:56:42 +0800 Subject: [PATCH 107/119] Use rclcpp for logging everywhere Signed-off-by: Arjo Chakravarty --- .../internal_ReservationNodeNegotiator.hpp | 3 +- rmf_reservation_node/src/main.cpp | 39 ++++++++++++------- 2 files changed, 28 insertions(+), 14 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index c9c3e4435..9d8357eda 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -109,7 +109,7 @@ class ReservationNodeNegotiator : { RCLCPP_ERROR( self->_context->node()->get_logger(), - "Reservations: Got no waitpoints"); + "Reservations: Got no waitpoints for %s", self->_context->requester_id().c_str()); return; } @@ -156,6 +156,7 @@ class ReservationNodeNegotiator : if (self->_current_reservation_state == ReservationState::Requested) { + /// Release all previous reservations if a new reservation was requested. self->force_release(); } diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index 553847459..84fae13b6 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -135,12 +135,15 @@ class CurrentState return locations; } - void add_location(std::string location) + void add_location(std::shared_ptr node, std::string location) { if (location.empty()) { - std::cerr << "Got an empty location name. Make sure all locations" - << " are set correctly" << std::endl; + std::stringstream str; + + + RCLCPP_ERROR(node->get_logger(), + "Got an empty location name. Make sure all locations have names."); return; } if (_current_location_reservations.count(location) == 0) @@ -150,8 +153,9 @@ class CurrentState } else { - std::cerr << "Got duplicate location [" << location - << "]" << std::endl; + + RCLCPP_ERROR(node->get_logger(), + "Got duplicate location: [%s]", location.c_str()); } } @@ -160,6 +164,7 @@ class CurrentState /// \param[in] incoming_requests - Parking spot and cost of said parking spot. /// \param[in] ticket_id - Ticket which is being serviced. std::optional allocate_lowest_cost_free_spot( + std::shared_ptr node, std::vector requests, const std::size_t ticket_id) { @@ -196,13 +201,14 @@ class CurrentState } } - std::cerr << "Could not find a free space from any of: "; + std::stringstream str; + str << "Could not find a free space from any of: "; for (auto c: requests) { - std::cerr << c.location << ", "; + str << c.location << ", "; } - std::cerr << "\n"; - + RCLCPP_ERROR(node->get_logger(), + "%s", str.str().c_str()); return std::nullopt; } @@ -286,6 +292,8 @@ class ItemQueue /// come in we simultaneously add them to every queue which can be serviced. /// Once a resource becomes free we call `service_next_in_queue` for said resource. /// When we service the next item in the queue we remove it from all other queues. +/// We essentially use this to reconstruct a waitgraph that we free when an item is +/// released. class ServiceQueueManager { std::unordered_mapvertices[i].name); + current_state_.add_location(shared_from_this(), + graph_msg->vertices[i].name); } } } @@ -419,7 +428,9 @@ class ReservationNode : public rclcpp::Node } // Allocate the lowest cost free spot from list of intended final locations if possible - auto result = current_state_.allocate_lowest_cost_free_spot(locations, + auto result = current_state_.allocate_lowest_cost_free_spot( + shared_from_this(), + locations, request->ticket.ticket_id); if (result.has_value()) { @@ -469,6 +480,7 @@ class ReservationNode : public rclcpp::Node waitpoints_[request->ticket.ticket_id] = wait_points; auto waitpoint_result = current_state_.allocate_lowest_cost_free_spot( + shared_from_this(), wait_points, request->ticket.ticket_id); if (waitpoint_result.has_value()) @@ -529,8 +541,9 @@ class ReservationNode : public rclcpp::Node } auto result = - current_state_.allocate_lowest_cost_free_spot(requests_[next_ticket. - value()], + current_state_.allocate_lowest_cost_free_spot( + shared_from_this(), + requests_[next_ticket.value()], next_ticket.value()); RCLCPP_DEBUG( this->get_logger(), "Found next item %lu on queue %s", From ed07cb86c6e8f20bb711a167b2d6a305684434c4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 2 Oct 2024 11:47:56 +0800 Subject: [PATCH 108/119] Cancellation flow Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/StandardNames.hpp | 1 + .../src/rmf_fleet_adapter/agv/Node.cpp | 9 ++++ .../src/rmf_fleet_adapter/agv/Node.hpp | 6 +++ .../events/EmergencyPullover.cpp | 4 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 11 +---- .../internal_ReservationNodeNegotiator.hpp | 12 ++++- rmf_reservation_node/src/main.cpp | 48 +++++++++++++------ 7 files changed, 64 insertions(+), 27 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 3eb2af9c6..fc40d2772 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -82,6 +82,7 @@ const std::string ReservationClaimTopicName = "rmf/reservations/claim"; const std::string ReservationAllocationTopicName = "rmf/reservations/allocation"; const std::string ReservationReleaseTopicName = "rmf/reservations/release"; +const std::string ReservationCancelTopicName = "rmf/reservations/cancel"; const uint64_t Unclaimed = (uint64_t)(-1); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 15faa2c83..22f1f216b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -139,6 +139,10 @@ std::shared_ptr Node::make( node->create_publisher( ReservationReleaseTopicName, transient_local_qos); + node->_reservation_cancel_pub = + node->create_publisher( + ReservationCancelTopicName, transient_local_qos); + return node; } @@ -310,5 +314,10 @@ auto Node::release_location() const -> const ReservationReleasePub& return _reservation_release_pub; } +//============================================================================== +auto Node::cancel_reservation() const -> const ReservationCancelPub& +{ + return _reservation_cancel_pub; +} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 1af3d8333..4e0c45e33 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -166,6 +166,11 @@ class Node : public rmf_rxcpp::Transport rclcpp::Publisher::SharedPtr; const ReservationReleasePub& release_location() const; + using ReservationCancel = rmf_reservation_msgs::msg::ReleaseRequest; + using ReservationCancelPub = + rclcpp::Publisher::SharedPtr; + const ReservationCancelPub& cancel_reservation() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( @@ -232,6 +237,7 @@ class Node : public rmf_rxcpp::Transport ReservationClaimPub _reservation_claim_pub; Bridge _reservation_alloc_obs; ReservationReleasePub _reservation_release_pub; + ReservationCancelPub _reservation_cancel_pub; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index c53f2811b..59eb3b68e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -264,7 +264,7 @@ void EmergencyPullover::Active::cancel() _state->update_log().info("Received signal to cancel"); if (_context->_parking_spot_manager_enabled()) { - _reservation_client->force_release(); + _reservation_client->cancel(); } _finished(); } @@ -277,7 +277,7 @@ void EmergencyPullover::Active::kill() _state->update_log().info("Received signal to kill"); if (_context->_parking_spot_manager_enabled()) { - _reservation_client->force_release(); + _reservation_client->cancel(); } _finished(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 58bd04b53..b6297f1cb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -432,7 +432,7 @@ void GoToPlace::Active::cancel() if (_context->_parking_spot_manager_enabled()) { - _reservation_client->force_release(); + _reservation_client->cancel(); } _finished(); } @@ -445,7 +445,7 @@ void GoToPlace::Active::kill() _state->update_log().info("Received signal to kill"); if (_context->_parking_spot_manager_enabled()) { - _reservation_client->force_release(); + _reservation_client->cancel(); } _finished(); } @@ -754,9 +754,6 @@ void GoToPlace::Active::_execute_plan( std::move(full_itinerary), _assign_id, _state, _update, [&]() { - RCLCPP_INFO( - _context->node()->get_logger(), - "reservation: Finished execution"); _stop_and_clear(); _finished(); }, _tail_period); @@ -768,10 +765,6 @@ void GoToPlace::Active::_execute_plan( std::move(full_itinerary), _assign_id, _state, _update, [&]() { - RCLCPP_INFO( - _context->node()->get_logger(), - "reservation: Reached waitpoint"); - _reached_waitpoint = true; }, _tail_period); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index 9d8357eda..cb6c371fa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -45,6 +45,16 @@ class ReservationNodeNegotiator : _reservation_id = _context->last_reservation_request_id(); } + void cancel() + { + if (auto allocation = _context->_release_resource()) + { + rmf_reservation_msgs::msg::ReleaseRequest msg; + msg.ticket = allocation->ticket; + _context->node()->cancel_reservation()->publish(msg); + } + } + void force_release() { std::unordered_set released_ticket_ids; @@ -90,7 +100,7 @@ class ReservationNodeNegotiator : return; } - RCLCPP_INFO( + RCLCPP_DEBUG( self->_context->node()->get_logger(), "Reservations: Got ticket issueing claim"); diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index 84fae13b6..cf02ee533 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -308,10 +308,7 @@ class ServiceQueueManager return std::nullopt; } - for (auto& [_, resource_queue]: resource_queues) - { - resource_queue.remove_item(item.value()); - } + remove_ticket(item.value()); return item; } @@ -323,6 +320,14 @@ class ServiceQueueManager resource_queues[resource].add(ticket); } } + + void remove_ticket(std::size_t ticket) + { + for (auto& [_, resource_queue]: resource_queues) + { + resource_queue.remove_item(ticket); + } + } }; using namespace std::chrono_literals; @@ -341,23 +346,27 @@ class ReservationNode : public rclcpp::Node request_subscription_ = this->create_subscription( - ReservationRequestTopicName, qos, - std::bind(&ReservationNode::on_request, this, - std::placeholders::_1)); + ReservationRequestTopicName, qos, + std::bind(&ReservationNode::on_request, this, + std::placeholders::_1)); claim_subscription_ = this->create_subscription( - ReservationClaimTopicName, qos, - std::bind(&ReservationNode::claim_request, this, - std::placeholders::_1)); + ReservationClaimTopicName, qos, + std::bind(&ReservationNode::claim_request, this, + std::placeholders::_1)); release_subscription_ = this->create_subscription( - ReservationReleaseTopicName, qos, - std::bind(&ReservationNode::release, this, std::placeholders::_1)); + ReservationReleaseTopicName, qos, + std::bind(&ReservationNode::release, this, std::placeholders::_1)); + cancel_subscription_ = + this->create_subscription( + ReservationCancelTopicName, qos, + std::bind(&ReservationNode::cancel, this, std::placeholders::_1)); graph_subscription_ = this->create_subscription( - "/nav_graphs", qos, - std::bind(&ReservationNode::received_graph, this, - std::placeholders::_1)); + NavGraphTopicName, qos, + std::bind(&ReservationNode::received_graph, this, + std::placeholders::_1)); ticket_pub_ = this->create_publisher( ReservationResponseTopicName, qos); @@ -505,6 +514,13 @@ class ReservationNode : public rclcpp::Node } } + void cancel( + const rmf_reservation_msgs::msg::ReleaseRequest::ConstSharedPtr& request) + { + queue_manager_.remove_ticket(request->ticket.ticket_id); + release(request); + } + void release( const rmf_reservation_msgs::msg::ReleaseRequest::ConstSharedPtr& request) { @@ -596,6 +612,8 @@ class ReservationNode : public rclcpp::Node claim_subscription_; rclcpp::Subscription::SharedPtr release_subscription_; + rclcpp::Subscription::SharedPtr + cancel_subscription_; rclcpp::Subscription::SharedPtr graph_subscription_; From 3d25ec74b5f1b8423361f54725aedb006984374d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 2 Oct 2024 13:02:04 +0800 Subject: [PATCH 109/119] Make sure cancellation is done on worker Signed-off-by: Arjo Chakravarty --- .../internal_ReservationNodeNegotiator.hpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index cb6c371fa..260cf5a0e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -47,12 +47,23 @@ class ReservationNodeNegotiator : void cancel() { - if (auto allocation = _context->_release_resource()) + _context->worker().schedule( + [ptr = weak_from_this()]( + const auto&) { + auto self = ptr.lock(); + if(!self) + { + return; + } + if (!self->_ticket.has_value()) + { + return; + } rmf_reservation_msgs::msg::ReleaseRequest msg; - msg.ticket = allocation->ticket; - _context->node()->cancel_reservation()->publish(msg); - } + msg.ticket = *self->_ticket.value().get(); + self->_context->node()->cancel_reservation()->publish(msg); + }); } void force_release() From 9e16a6e7985f97432c7a01c0ffe29b305a66b368 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 3 Oct 2024 12:56:18 +0800 Subject: [PATCH 110/119] Use updated topic names Signed-off-by: Arjo Chakravarty --- .../events/internal_ReservationNodeNegotiator.hpp | 10 +++++----- rmf_reservation_node/src/main.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index 260cf5a0e..5bc5db13c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -189,26 +189,26 @@ class ReservationNodeNegotiator : "Reservation: Robot %s is going to final destination %lu", self->_context->name().c_str(), self->_goals[self->_final_allocated_destination.value()-> - satisfies_alternative].waypoint()); + chosen_alternative].waypoint()); self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; self->_selected_final_destination_cb(self->_goals[self-> _final_allocated_destination.value()-> - satisfies_alternative].waypoint()); + chosen_alternative].waypoint()); } if (msg->instruction_type - == rmf_reservation_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY) + == rmf_reservation_msgs::msg::ReservationAllocation::WAIT_IDENTIFIED) { self->_current_reservation_state = ReservationState::ReceivedResponseProceedWaitPoint; self->_selected_waitpoint_cb(self->_waitpoints[self-> _final_allocated_destination.value()-> - satisfies_alternative]); + chosen_alternative]); RCLCPP_INFO( self->_context->node()->get_logger(), "Reservation: Robot %s is being asked to proceed to a waitpoint %lu", self->_context->name().c_str(), self->_waitpoints[self->_final_allocated_destination.value()-> - satisfies_alternative].waypoint()); + chosen_alternative].waypoint()); } }); diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index cf02ee533..e750dc6a9 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -448,7 +448,7 @@ class ReservationNode : public rclcpp::Node request->ticket.ticket_id); allocation.instruction_type = rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; - allocation.satisfies_alternative = result.value(); + allocation.chosen_alternative = result.value(); allocation.resource = requests_[request->ticket.ticket_id][result.value()].location; @@ -498,8 +498,8 @@ class ReservationNode : public rclcpp::Node allocation.ticket = ticket_store_.get_existing_ticket( request->ticket.ticket_id); allocation.instruction_type = - rmf_reservation_msgs::msg::ReservationAllocation::WAIT_PERMANENTLY; - allocation.satisfies_alternative = waitpoint_result.value(); + rmf_reservation_msgs::msg::ReservationAllocation::WAIT_IDENTIFIED; + allocation.chosen_alternative = waitpoint_result.value(); allocation.resource = wait_points[waitpoint_result.value()].location; RCLCPP_INFO(this->get_logger(), "Allocating %s as waitpoint to %s", allocation.resource.c_str(), @@ -577,7 +577,7 @@ class ReservationNode : public rclcpp::Node return; } rmf_reservation_msgs::msg::ReservationAllocation allocation; - allocation.satisfies_alternative = result.value(); + allocation.chosen_alternative = result.value(); allocation.resource = requests_[next_ticket.value()][result.value()].location; allocation.ticket = From d50e194f297bad07bc3d9e74d9b18b32ebda7fe4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 09:44:01 +0800 Subject: [PATCH 111/119] Clean up reservatiion Manager Signed-off-by: Arjo Chakravarty --- .../agv/ReservationManager.cpp | 72 +++++++++++++++++++ .../agv/ReservationManager.hpp | 33 ++++----- .../rmf_fleet_adapter/agv/RobotContext.cpp | 11 ++- .../rmf_fleet_adapter/agv/RobotContext.hpp | 11 ++- .../internal_ReservationNodeNegotiator.hpp | 42 ++--------- 5 files changed, 106 insertions(+), 63 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp new file mode 100644 index 000000000..d78ce4728 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2024 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 "ReservationManager.hpp" +#include "RobotContext.hpp" + +using namespace rmf_fleet_adapter::agv; + +//============================================================================== +void ReservationManager::add_ticket( + const rmf_reservation_msgs::msg::ReservationAllocation new_allocation) +{ + if (has_ticket()) + { + if (new_allocation.ticket.ticket_id != _allocation->ticket.ticket_id) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Releasing waypoint for ticket %lu as new ticket has become available", + _allocation->ticket.ticket_id); + rmf_reservation_msgs::msg::ReleaseRequest msg; + msg.ticket = _allocation->ticket; + _context->node()->release_location()->publish(msg); + } + } + _allocation = new_allocation; +} + +//============================================================================== +void ReservationManager::cancel() +{ + if (has_ticket()) + return; + + RCLCPP_INFO( + _context->node()->get_logger(), + "Cancelling ticket %lu", + _allocation->ticket.ticket_id); + rmf_reservation_msgs::msg::ReleaseRequest msg; + msg.ticket = _allocation->ticket; + _context->node()->release_location()->publish(msg); + _allocation = std::nullopt; +} + +//============================================================================== +std::string ReservationManager::get_reserved_location() const +{ +if (has_ticket()) + return _allocation->resource; + +return ""; +} + +//============================================================================== +bool ReservationManager::has_ticket() const +{ +return _allocation.has_value(); +} \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index bf378e7cc..ec0f56b0a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -24,33 +24,26 @@ namespace rmf_fleet_adapter { namespace agv { +class RobotContext; class ReservationManager { public: - void add_ticket(const rmf_reservation_msgs::msg::ReservationAllocation alloc) - { - allocations.push_front(alloc); - } + /// Adds a ticket and releases the previous ticket if the ticket id is different + void add_ticket( + const rmf_reservation_msgs::msg::ReservationAllocation new_allocation); - std::optional release_ticket() - { - if (allocations.size() <= 1) - { - // For safety every robot must have at least one reservation at any point in time. - return std::nullopt; - } - auto temp = allocations.back(); - allocations.pop_back(); - return temp; - } + /// Cancels the current reservation + void cancel(); + std::string get_reserved_location() const; - bool has_ticket() const - { - return allocations.size() != 0; - } + /// Checks if a ticket exists + bool has_ticket() const; +private: + std::optional _allocation; + std::shared_ptr _context; - std::deque allocations; + friend RobotContext; }; } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 7746b4410..9588775d7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1824,10 +1824,15 @@ bool RobotContext::_parking_spot_manager_enabled() } //============================================================================== -std::optional -RobotContext::_release_resource() +void RobotContext::_cancel_allocated_destination() { - return _reservation_mgr.release_ticket(); + return _reservation_mgr.cancel(); +} + +//============================================================================== +std::string RobotContext::_get_reserved_location() +{ + return _reservation_mgr.get_reserved_location(); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index d93df01f1..397d4365a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -783,6 +783,12 @@ class RobotContext void _set_allocated_destination( const rmf_reservation_msgs::msg::ReservationAllocation&); + /// Cancel allocated destination + void _cancel_allocated_destination(); + + /// Get last reserved location. Empty string if not reserved. + std::string _get_reserved_location(); + /// Set if the parking spot manager is used or not void _set_parking_spot_manager(const bool enabled); @@ -795,9 +801,6 @@ class RobotContext /// Set if the parking spot manager is used or not bool _parking_spot_manager_enabled(); - /// Release last resource that was acquired. - std::optional _release_resource(); - /// Has ticket now bool _has_ticket() const; @@ -865,6 +868,8 @@ class RobotContext self->_handle_mutex_group_manual_release(*msg); }); + context->_reservation_mgr._context = context; + return context; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index 5bc5db13c..58fe03861 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -19,7 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_RESERVATION_NEGOTIATOR_HPP #include "../agv/RobotContext.hpp" - +#include "internal_utilities.hpp" #include namespace rmf_fleet_adapter { @@ -56,35 +56,9 @@ class ReservationNodeNegotiator : { return; } - if (!self->_ticket.has_value()) - { - return; - } - rmf_reservation_msgs::msg::ReleaseRequest msg; - msg.ticket = *self->_ticket.value().get(); - self->_context->node()->cancel_reservation()->publish(msg); - }); - } - void force_release() - { - std::unordered_set released_ticket_ids; - while (auto allocation = _context->_release_resource()) - { - if (released_ticket_ids.count(allocation->ticket.ticket_id) != 0) - { - continue; - } - released_ticket_ids.insert(allocation->ticket.ticket_id); - rmf_reservation_msgs::msg::ReleaseRequest msg; - msg.ticket = allocation->ticket; - _context->node()->release_location()->publish(msg); - RCLCPP_INFO( - _context->node()->get_logger(), - "reservation: Releasing waypoint for ticket %lu", - msg.ticket.ticket_id - ); - } + self->_context->_cancel_allocated_destination(); + }); } static std::shared_ptr make( @@ -175,12 +149,6 @@ class ReservationNodeNegotiator : self->_final_allocated_destination = msg; self->_context->_set_allocated_destination(*msg.get()); - if (self->_current_reservation_state == ReservationState::Requested) - { - /// Release all previous reservations if a new reservation was requested. - self->force_release(); - } - if (msg->instruction_type == rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) { @@ -246,7 +214,7 @@ class ReservationNodeNegotiator : } for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) { - if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) + if (events::wp_name(*negotiator->_context.get(), negotiator->_goals[i]) == negotiator->_context->_get_reserved_location()) { RCLCPP_ERROR(negotiator->_context->node()->get_logger(), "Already at goal no need to engage reservation system\n"); @@ -263,7 +231,7 @@ class ReservationNodeNegotiator : for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) { - if (negotiator->_goals[i].waypoint() == current_location[0].waypoint()) + if (events::wp_name(*context.get(), negotiator->_goals[i]) == context->_get_reserved_location()) { RCLCPP_ERROR(context->node()->get_logger(), "Already at goal no need to engage reservation system\n"); From e68733f0e8616efada0663ba85fe3b0195348e89 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 13:54:01 +0800 Subject: [PATCH 112/119] Minor renames Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/ReservationManager.cpp | 4 ++-- .../rmf_fleet_adapter/agv/ReservationManager.hpp | 5 ++++- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 2 +- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 2 +- .../rmf_fleet_adapter/phases/WaitForCharge.cpp | 16 +++++++++++++++- .../rmf_fleet_adapter/phases/WaitForCharge.hpp | 3 ++- 6 files changed, 25 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp index d78ce4728..3f37e259a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp @@ -21,7 +21,7 @@ using namespace rmf_fleet_adapter::agv; //============================================================================== -void ReservationManager::add_ticket( +void ReservationManager::replace_ticket( const rmf_reservation_msgs::msg::ReservationAllocation new_allocation) { if (has_ticket()) @@ -52,7 +52,7 @@ void ReservationManager::cancel() _allocation->ticket.ticket_id); rmf_reservation_msgs::msg::ReleaseRequest msg; msg.ticket = _allocation->ticket; - _context->node()->release_location()->publish(msg); + _context->node()->cancel_reservation()->publish(msg); _allocation = std::nullopt; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index ec0f56b0a..5d928d23a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -30,11 +30,14 @@ class ReservationManager { public: /// Adds a ticket and releases the previous ticket if the ticket id is different - void add_ticket( + void replace_ticket( const rmf_reservation_msgs::msg::ReservationAllocation new_allocation); /// Cancels the current reservation void cancel(); + + /// Retrieves the location name of the current reservation. Returns empty string if + /// no location is found. std::string get_reserved_location() const; /// Checks if a ticket exists diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 9588775d7..3898d3d76 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1808,7 +1808,7 @@ void RobotContext::_publish_mutex_group_requests() void RobotContext::_set_allocated_destination( const rmf_reservation_msgs::msg::ReservationAllocation& ticket) { - _reservation_mgr.add_ticket(ticket); + _reservation_mgr.replace_ticket(ticket); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 397d4365a..b2a1ad7a1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -801,7 +801,7 @@ class RobotContext /// Set if the parking spot manager is used or not bool _parking_spot_manager_enabled(); - /// Has ticket now + /// Does the parking spot have a ticket? bool _has_ticket() const; template diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index be9c81c67..9139d20ca 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -16,7 +16,7 @@ */ #include "WaitForCharge.hpp" - +#include "../events/internal_ReservationNodeNegotiator.hpp" namespace rmf_fleet_adapter { namespace phases { @@ -118,6 +118,20 @@ WaitForCharge::Active::Active( retain_mutexes.begin()->c_str()); } +_reservation_client = std::move(reservation::ReservationNodeNegotiator::make( + _context, + std::vector{ charging_waypoint}, + true, + [](const rmf_traffic::agv::Plan::Goal& goal) + { + // Do Nothing + }, + [](const rmf_traffic::agv::Plan::Goal& goal) + { + // Do Nothing + } + )); + _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_CHARGING); _lock_charging = _context->be_charging(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp index 2c03adb62..2c99aabdb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -20,6 +20,7 @@ #include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" +#include "../events/internal_ReservationNodeNegotiator.hpp" #include @@ -77,7 +78,7 @@ class WaitForCharge double _initial_battery_soc; double _expected_charging_rate; // % per hour std::shared_ptr _lock_charging; - + std::shared_ptr _reservation_client; }; class Pending : public LegacyTask::PendingPhase From 6373d8cb5fe70e3fadc463f1893fb1b23b47db66 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 14:39:11 +0800 Subject: [PATCH 113/119] Fix compile error and rename variable to be more readable Signed-off-by: Arjo Chakravarty --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 4 ++-- rmf_reservation_node/src/main.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index 1a7e2f76c..d5a24a98e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -3387,7 +3387,7 @@ auto EasyFullControl::add_robot( localization = std::move(localization), nav_params = robot_nav_params, enable_responsive_wait, - use_parking_reservation = _pimpl->use_parking_reservation + use_parking_reservation = _pimpl->use_parking_reservation, finishing_request ](const RobotUpdateHandlePtr& updater) { @@ -3408,7 +3408,7 @@ auto EasyFullControl::add_robot( context, nav_params, enable_responsive_wait, - use_parking_reservation + use_parking_reservation, finishing_request ](const auto&) { diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index e750dc6a9..b6b8e369f 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -528,8 +528,8 @@ class ReservationNode : public rclcpp::Node this->get_logger(), "Releasing ticket for %s", ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); auto ticket = request->ticket.ticket_id; - auto released_location = current_state_.release(ticket); - if (!released_location.has_value()) + auto previous_waiting_location = current_state_.release(ticket); + if (!previous_waiting_location.has_value()) { RCLCPP_ERROR( this->get_logger(), "Could not find ticket %lu. Something is wrong.", @@ -539,15 +539,15 @@ class ReservationNode : public rclcpp::Node RCLCPP_DEBUG( this->get_logger(), "Released ticket %lu, location %s is now free", request->ticket.ticket_id, - released_location->c_str()); + previous_waiting_location->c_str()); // Traverse waitgraph. while (auto next_ticket = - queue_manager_.service_next_in_queue(released_location.value())) + queue_manager_.service_next_in_queue(previous_waiting_location.value())) { // Release the ticket - released_location = current_state_.release(next_ticket.value()); - if (!released_location.has_value()) + previous_waiting_location = current_state_.release(next_ticket.value()); + if (!previous_waiting_location.has_value())u { RCLCPP_ERROR( this->get_logger(), @@ -564,7 +564,7 @@ class ReservationNode : public rclcpp::Node RCLCPP_DEBUG( this->get_logger(), "Found next item %lu on queue %s", next_ticket.value(), - released_location.value().c_str()); + previous_waiting_location.value().c_str()); if (!result.has_value()) @@ -593,7 +593,7 @@ class ReservationNode : public rclcpp::Node } RCLCPP_DEBUG( this->get_logger(), "Queue is now empty %s", - released_location->c_str()); + previous_waiting_location->c_str()); return; } From d016fbeb81b41b6672dabf01c89f9199f7bd7b56 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 14:46:40 +0800 Subject: [PATCH 114/119] Remove key stroke Signed-off-by: Arjo Chakravarty --- rmf_reservation_node/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp index b6b8e369f..0d5429432 100644 --- a/rmf_reservation_node/src/main.cpp +++ b/rmf_reservation_node/src/main.cpp @@ -547,7 +547,7 @@ class ReservationNode : public rclcpp::Node { // Release the ticket previous_waiting_location = current_state_.release(next_ticket.value()); - if (!previous_waiting_location.has_value())u + if (!previous_waiting_location.has_value()) { RCLCPP_ERROR( this->get_logger(), From d610e13944a67d613b87badf3b6281beb3550028 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 15:10:04 +0800 Subject: [PATCH 115/119] Fix logging levels and log robot id Signed-off-by: Arjo Chakravarty --- .../events/internal_ReservationNodeNegotiator.hpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index 58fe03861..da389d410 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -216,14 +216,16 @@ class ReservationNodeNegotiator : { if (events::wp_name(*negotiator->_context.get(), negotiator->_goals[i]) == negotiator->_context->_get_reserved_location()) { - RCLCPP_ERROR(negotiator->_context->node()->get_logger(), - "Already at goal no need to engage reservation system\n"); + RCLCPP_INFO(negotiator->_context->node()->get_logger(), + "%s: Already at goal no need to engage reservation system\n", + negotiator->_context->requester_id().c_str()); negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); return; } } RCLCPP_INFO(negotiator->_context->node()->get_logger(), - "Sending reservation request"); + "%s: Sending reservation request", + negotiator->_context->requester_id().c_str()); negotiator->make_request(same_map); } ); @@ -233,8 +235,9 @@ class ReservationNodeNegotiator : { if (events::wp_name(*context.get(), negotiator->_goals[i]) == context->_get_reserved_location()) { - RCLCPP_ERROR(context->node()->get_logger(), - "Already at goal no need to engage reservation system\n"); + RCLCPP_INFO(context->node()->get_logger(), + "%s: Already at goal no need to engage reservation system\n", + context->requester_id().c_str()); negotiator->_selected_final_destination_cb(negotiator->_goals[i]); return; } From 0b8e7fb030e608dcca1272cb499048a07f11f89d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 15:26:22 +0800 Subject: [PATCH 116/119] Remove retry timer. Signed-off-by: Arjo Chakravarty --- .../internal_ReservationNodeNegotiator.hpp | 56 +++---------------- 1 file changed, 7 insertions(+), 49 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index da389d410..cc3b0ed48 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -190,60 +190,20 @@ class ReservationNodeNegotiator : auto negotiator = ptr.lock(); - auto current_location = context->location(); - if (current_location.size() == 0) - { - using namespace std::literals::chrono_literals; - negotiator->_retry_timer = context->node()->create_wall_timer( - 500ms, [self = negotiator->weak_from_this(), same_map]() - { - auto negotiator = self.lock(); - if(!negotiator) - { - return; - } - - auto current_location = negotiator->_context->location(); - if (current_location.size() != 0) - { - negotiator->_retry_timer->cancel(); - } - else - { - return; - } - for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) - { - if (events::wp_name(*negotiator->_context.get(), negotiator->_goals[i]) == negotiator->_context->_get_reserved_location()) - { - RCLCPP_INFO(negotiator->_context->node()->get_logger(), - "%s: Already at goal no need to engage reservation system\n", - negotiator->_context->requester_id().c_str()); - negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); - return; - } - } - RCLCPP_INFO(negotiator->_context->node()->get_logger(), - "%s: Sending reservation request", - negotiator->_context->requester_id().c_str()); - negotiator->make_request(same_map); - } - ); - } - for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) { - if (events::wp_name(*context.get(), negotiator->_goals[i]) == context->_get_reserved_location()) + if (events::wp_name(*negotiator->_context.get(), negotiator->_goals[i]) == negotiator->_context->_get_reserved_location()) { - RCLCPP_INFO(context->node()->get_logger(), + RCLCPP_INFO(negotiator->_context->node()->get_logger(), "%s: Already at goal no need to engage reservation system\n", - context->requester_id().c_str()); - negotiator->_selected_final_destination_cb(negotiator->_goals[i]); + negotiator->_context->requester_id().c_str()); + negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); return; } } - RCLCPP_INFO(context->node()->get_logger(), - "Sending reservation request"); + RCLCPP_INFO(negotiator->_context->node()->get_logger(), + "%s: Sending reservation request", + negotiator->_context->requester_id().c_str()); negotiator->make_request(same_map); }); return negotiator; @@ -251,8 +211,6 @@ class ReservationNodeNegotiator : private: - rclcpp::TimerBase::SharedPtr _retry_timer; - enum class ReservationState { Pending=0, From 7efbf4586ef974c5775327977d2fce53061bf97e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Oct 2024 15:44:59 +0800 Subject: [PATCH 117/119] Refactor Signed-off-by: Arjo Chakravarty --- .../internal_ReservationNodeNegotiator.hpp | 44 +++++++++++-------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp index cc3b0ed48..467fe0b82 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -180,30 +180,36 @@ class ReservationNodeNegotiator : } }); + + for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) + { + if (events::wp_name(*context.get(), negotiator->_goals[i]) == context->_get_reserved_location()) + { + RCLCPP_INFO(context->node()->get_logger(), + "%s: Already at goal no need to engage reservation system\n", + context->requester_id().c_str()); + context->worker().schedule([ + cb = negotiator->_selected_final_destination_cb, + wp = negotiator->_goals[i] + ](const auto&) + { + cb(wp); + }); + return negotiator; + } + } + RCLCPP_INFO(negotiator->_context->node()->get_logger(), + "%s: Sending reservation request", + negotiator->_context->requester_id().c_str()); + context->worker().schedule( - [ptr = negotiator->weak_from_this(), w = context->weak_from_this(), same_map]( + [ptr = negotiator->weak_from_this(), same_map]( const auto&) { - auto context = w.lock(); - if (!context) - return; - auto negotiator = ptr.lock(); + if (!negotiator) + return; - for (std::size_t i = 0; i < negotiator->_goals.size(); ++i) - { - if (events::wp_name(*negotiator->_context.get(), negotiator->_goals[i]) == negotiator->_context->_get_reserved_location()) - { - RCLCPP_INFO(negotiator->_context->node()->get_logger(), - "%s: Already at goal no need to engage reservation system\n", - negotiator->_context->requester_id().c_str()); - negotiator->_selected_final_destination_cb(negotiator->_goals[i].waypoint()); - return; - } - } - RCLCPP_INFO(negotiator->_context->node()->get_logger(), - "%s: Sending reservation request", - negotiator->_context->requester_id().c_str()); negotiator->make_request(same_map); }); return negotiator; From 1470440ed8a15e884f08281cd37fe6ac75649e29 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 5 Oct 2024 13:28:10 +0800 Subject: [PATCH 118/119] Fixed memory leak Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/CMakeLists.txt | 4 ++++ .../agv/ReservationManager.cpp | 24 +++++++++++++------ .../agv/ReservationManager.hpp | 2 +- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index ef83cb60c..6c2f0dfbe 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -38,6 +38,7 @@ set(dep_pkgs rmf_battery rmf_task rmf_task_sequence + rmf_reservation_msgs std_msgs rmf_api_msgs rmf_websocket @@ -88,6 +89,7 @@ ament_target_dependencies(rmf_fleet_adapter rmf_dispenser_msgs rmf_ingestor_msgs rmf_building_map_msgs + rmf_reservation_msgs rclcpp ) @@ -115,6 +117,7 @@ target_include_directories(rmf_fleet_adapter ${rmf_dispenser_msgs_INCLUDE_DIRS} ${rmf_ingestor_msgs_INCLUDE_DIRS} ${rmf_api_msgs_INCLUDE_DIRS} + ${rmf_reservation_msgs_INCLUDE_DIRS} ${rmf_websocket_INCLUDE_DIR} ${rmf_traffic_ros2_INCLUDE_DIRS} ${rmf_building_map_msgs_INCLUDE_DIRS} @@ -152,6 +155,7 @@ if (BUILD_TESTING) std_msgs rmf_building_map_msgs rmf_websocket + rmf_reservation_msgs ) target_include_directories(test_rmf_fleet_adapter PRIVATE diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp index 3f37e259a..3bbb8d9ed 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp @@ -24,17 +24,22 @@ using namespace rmf_fleet_adapter::agv; void ReservationManager::replace_ticket( const rmf_reservation_msgs::msg::ReservationAllocation new_allocation) { + auto context = _context.lock(); + if (!context) + { + return; + } if (has_ticket()) { if (new_allocation.ticket.ticket_id != _allocation->ticket.ticket_id) { RCLCPP_INFO( - _context->node()->get_logger(), + context->node()->get_logger(), "Releasing waypoint for ticket %lu as new ticket has become available", _allocation->ticket.ticket_id); rmf_reservation_msgs::msg::ReleaseRequest msg; msg.ticket = _allocation->ticket; - _context->node()->release_location()->publish(msg); + context->node()->release_location()->publish(msg); } } _allocation = new_allocation; @@ -43,30 +48,35 @@ void ReservationManager::replace_ticket( //============================================================================== void ReservationManager::cancel() { + auto context = _context.lock(); + if (!context) + { + return; + } if (has_ticket()) return; RCLCPP_INFO( - _context->node()->get_logger(), + context->node()->get_logger(), "Cancelling ticket %lu", _allocation->ticket.ticket_id); rmf_reservation_msgs::msg::ReleaseRequest msg; msg.ticket = _allocation->ticket; - _context->node()->cancel_reservation()->publish(msg); + context->node()->cancel_reservation()->publish(msg); _allocation = std::nullopt; } //============================================================================== std::string ReservationManager::get_reserved_location() const { -if (has_ticket()) + if (has_ticket()) return _allocation->resource; -return ""; + return ""; } //============================================================================== bool ReservationManager::has_ticket() const { -return _allocation.has_value(); + return _allocation.has_value(); } \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp index 5d928d23a..1ae22c22d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -44,7 +44,7 @@ class ReservationManager bool has_ticket() const; private: std::optional _allocation; - std::shared_ptr _context; + std::weak_ptr _context; friend RobotContext; }; From c033f4b71b78191f015eda3e644d08b7ab40ac45 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 5 Oct 2024 13:47:23 +0800 Subject: [PATCH 119/119] An excellent example of something most modern languages would have prevented. Signed-off-by: Arjo Chakravarty --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 67ebca734..161f3622b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1296,7 +1296,8 @@ RobotContext::RobotContext( _current_task_id(std::nullopt), _task_planner(std::move(task_planner)), _reporting(_worker), - _last_reservation_request_id(0) + _last_reservation_request_id(0), + _use_parking_spot_reservations(false) { _most_recent_valid_location = _location; _profile = std::make_shared(