diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index ef83cb60..6c2f0dfb 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/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 5915541b..54871900 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -80,6 +80,14 @@ 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 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); } // namespace rmf_fleet_adapter 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 5068094a..a50123d0 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -776,6 +776,13 @@ 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 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. void set_default_responsive_wait(bool enable); diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index dd3c3b2d..214a56fa 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -21,6 +21,7 @@ rmf_api_msgs rmf_battery rmf_building_map_msgs + rmf_reservation_msgs rmf_dispenser_msgs rmf_door_msgs rmf_fleet_msgs 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 ee49b354..a7150f37 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -383,7 +383,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.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 b1d8edd8..d5a24a98 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -2007,6 +2007,7 @@ class EasyFullControl::FleetConfiguration::Implementation double default_min_lane_length; std::unordered_map lift_emergency_levels; std::unordered_set strict_lanes; + bool use_parking_reservation; }; //============================================================================== @@ -2063,7 +2064,8 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(default_max_merge_lane_distance), std::move(default_min_lane_length), {}, - {} + {}, + false // Parking reservation system })) { // Do nothing @@ -2474,6 +2476,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"]; @@ -2743,6 +2753,7 @@ EasyFullControl::FleetConfiguration::from_config_files( 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; } @@ -3056,6 +3067,20 @@ bool EasyFullControl::FleetConfiguration::default_responsive_wait() const return _pimpl->default_responsive_wait; } +//============================================================================== +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) @@ -3362,6 +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, finishing_request ](const RobotUpdateHandlePtr& updater) { @@ -3382,6 +3408,7 @@ auto EasyFullControl::add_robot( context, nav_params, enable_responsive_wait, + use_parking_reservation, finishing_request ](const auto&) { @@ -3403,6 +3430,8 @@ auto EasyFullControl::add_robot( context->robot_finishing_request(true); } + context->_set_parking_spot_manager(use_parking_reservation); + RCLCPP_INFO( node->get_logger(), "Successfully added robot [%s] to the fleet [%s].", 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 cc596946..22f1f216 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -119,6 +119,30 @@ 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); + + node->_reservation_release_pub = + node->create_publisher( + ReservationReleaseTopicName, transient_local_qos); + + node->_reservation_cancel_pub = + node->create_publisher( + ReservationCancelTopicName, transient_local_qos); + return node; } @@ -260,5 +284,40 @@ 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(); +} + +//============================================================================== +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 5331b43e..4e0c45e3 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,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP #define SRC__RMF_FLEET_ADAPTER__AGV__NODE_HPP + #include #include @@ -32,6 +33,13 @@ #include #include #include +#include +#include +#include +#include +#include +#include + #include #include @@ -135,6 +143,35 @@ class Node : public rmf_rxcpp::Transport using MutexGroupStatesObs = rxcpp::observable; const MutexGroupStatesObs& mutex_group_states() const; + using ReservationRequest = rmf_reservation_msgs::msg::FlexibleTimeRequest; + using ReservationRequestPub = + rclcpp::Publisher::SharedPtr; + const ReservationRequestPub& location_requester() const; + + using ReservationTicket = rmf_reservation_msgs::msg::Ticket; + using ReservationTicketObs = rxcpp::observable; + const ReservationTicketObs& location_ticket_obs() const; + + using ReservationClaim = rmf_reservation_msgs::msg::ClaimRequest; + using ReservationClaimPub = rclcpp::Publisher::SharedPtr; + const ReservationClaimPub& claim_location_ticket() const; + + using ReservationAllocation = rmf_reservation_msgs::msg::ReservationAllocation; + using ReservationAllocationObs = + rxcpp::observable; + const ReservationAllocationObs& allocated_claims_obs() const; + + using ReservationRelease = rmf_reservation_msgs::msg::ReleaseRequest; + using ReservationReleasePub = + 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( std::chrono::duration period, @@ -195,6 +232,12 @@ 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; + ReservationReleasePub _reservation_release_pub; + ReservationCancelPub _reservation_cancel_pub; }; } // namespace agv 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 00000000..3bbb8d9e --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.cpp @@ -0,0 +1,82 @@ +/* + * 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::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(), + "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() +{ + auto context = _context.lock(); + if (!context) + { + return; + } + 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()->cancel_reservation()->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 new file mode 100644 index 00000000..1ae22c22 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/ReservationManager.hpp @@ -0,0 +1,53 @@ +/* + * 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__RESERVATION_MANAGER_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__RESERVATION_MANAGER_HPP +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace agv { + +class RobotContext; + +class ReservationManager +{ +public: + /// Adds a ticket and releases the previous ticket if the ticket id is different + 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 + bool has_ticket() const; +private: + std::optional _allocation; + std::weak_ptr _context; + + friend RobotContext; +}; +} +} +#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 caf007a1..161f3622 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -26,6 +26,8 @@ #include #include +#include +#include namespace rmf_fleet_adapter { namespace agv { @@ -997,6 +999,12 @@ 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, @@ -1287,7 +1295,9 @@ 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), + _use_parking_spot_reservations(false) { _most_recent_valid_location = _location; _profile = std::make_shared( @@ -1807,6 +1817,120 @@ void RobotContext::_publish_mutex_group_requests() } } +//============================================================================== +void RobotContext::_set_allocated_destination( + const rmf_reservation_msgs::msg::ReservationAllocation& ticket) +{ + _reservation_mgr.replace_ticket(ticket); +} + +//============================================================================== +void RobotContext::_set_parking_spot_manager(const bool enabled) +{ + _use_parking_spot_reservations = enabled; +} + +//============================================================================== +bool RobotContext::_parking_spot_manager_enabled() +{ + return _use_parking_spot_reservations; +} + +//============================================================================== +void RobotContext::_cancel_allocated_destination() +{ + return _reservation_mgr.cancel(); +} + +//============================================================================== +std::string RobotContext::_get_reserved_location() +{ + return _reservation_mgr.get_reserved_location(); +} + +//============================================================================== +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 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; + } + + // Order wait points by the distance from the destination. + std::vector> + waitpoints_order; + 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) + { + + // 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()) + { + RCLCPP_INFO( + node()->get_logger(), + "No path found for waypoint #%lu", + wp_idx); + 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( const rmf_fleet_msgs::msg::MutexGroupManualRelease& msg) 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 155b86f1..c16f5168 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -46,6 +46,7 @@ #include "Node.hpp" #include "../Reporting.hpp" +#include "ReservationManager.hpp" #include @@ -410,6 +411,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. @@ -783,6 +787,31 @@ 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_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); + + /// 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; + + /// Set if the parking spot manager is used or not + bool _parking_spot_manager_enabled(); + + /// Does the parking spot have a ticket? + bool _has_ticket() const; + template static std::shared_ptr make(Args&&... args) { @@ -847,6 +876,8 @@ class RobotContext self->_handle_mutex_group_manual_release(*msg); }); + context->_reservation_mgr._context = context; + return context; } @@ -969,6 +1000,10 @@ class RobotContext _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(); 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 83224d48..4cc50ab2 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,6 +36,7 @@ class EasyFullControl::Implementation std::unordered_map cmd_handles; NavParams nav_params; bool default_responsive_wait; + bool use_parking_reservation; static std::shared_ptr make( std::shared_ptr fleet_handle, @@ -45,7 +46,8 @@ class EasyFullControl::Implementation 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) { auto handle = std::shared_ptr(new EasyFullControl); handle->_pimpl = rmf_utils::make_unique_impl( @@ -60,7 +62,8 @@ class EasyFullControl::Implementation default_max_merge_lane_distance, default_min_lane_length, }, - default_responsive_wait + default_responsive_wait, + use_parking_reservation }); return handle; } 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 222df2d6..59eb3b68 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -22,6 +22,9 @@ #include #include #include +#include "../project_itinerary.hpp" + +#include "internal_utilities.hpp" namespace rmf_fleet_adapter { namespace events { @@ -155,7 +158,45 @@ 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 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 reservation negotiator"); + active->_reservation_client = reservation::ReservationNodeNegotiator::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; } @@ -221,6 +262,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()) + { + _reservation_client->cancel(); + } _finished(); } @@ -230,6 +275,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()) + { + _reservation_client->cancel(); + } _finished(); } @@ -242,66 +291,148 @@ 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->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) + 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) { - // 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"); + 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(); - auto full_itinerary = result->get_itinerary(); - self->_execute_plan( - self->_context->itinerary().assign_plan_id(), - *std::move(result), - std::move(full_itinerary)); + if (const auto self = weak_self.lock()) + self->_find_pullover_timeout = nullptr; + }); - self->_find_pullover_service = nullptr; - self->_retry_timer = nullptr; - }); + _update(); + } + else + { - _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(); + _find_path_service = std::make_shared( + _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.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.value()]( + const services::FindPath::Result& result) + { + const auto self = w.lock(); + if (!self) + return; + + 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."); + + // 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(); }); + + 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, {}, + *self->_context->planner()); + + self->_execute_plan( + self->_context->itinerary().assign_plan_id(), + *std::move(result), + std::move(full_itinerary)); + + self->_find_path_service = nullptr; + self->_retry_timer = 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(); - if (const auto self = weak_self.lock()) - self->_find_pullover_timeout = nullptr; - }); + if (const auto self = weak_self.lock()) + 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 6a4b4ca9..0a2dfcb1 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_ReservationNodeNegotiator.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 _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 846945bc..b6297f1c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -17,9 +17,16 @@ #include "GoToPlace.hpp" #include "../project_itinerary.hpp" +#include "internal_ReservationNodeNegotiator.hpp" +#include "internal_utilities.hpp" +#include "PerformAction.hpp" +#include +#include +#include #include #include +#include namespace rmf_fleet_adapter { namespace events { @@ -193,6 +200,12 @@ auto GoToPlace::Active::make( if (const auto c = self->_context->command()) c->stop(); + + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Goal selected %s", + wp_name(*self->_context, self->_chosen_goal.value()).c_str()); + self->_find_plan(); } }); @@ -260,7 +273,41 @@ 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 + { + // This is the parking spot manager. + active->_is_final_destination = false; + active->_chosen_goal = std::nullopt; + + active->_reservation_client = std::move(reservation::ReservationNodeNegotiator::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_reservation_node_allocate_final_destination(goal); + } + }, + [w = + active->weak_from_this()](const rmf_traffic::agv::Plan::Goal& goal) + { + if (auto self = w.lock()) + { + self->_on_reservation_node_allocate_waitpoint(goal); + } + } + )); + } + return active; } @@ -270,6 +317,31 @@ auto GoToPlace::Active::state() const -> ConstStatePtr return _state; } +//============================================================================== +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 reservation node", + _context->requester_id().c_str(), + wp_name(*_context, goal).c_str()); + _is_final_destination = true; + + _chosen_goal = goal; + _find_plan(); +} + +//============================================================================== +void GoToPlace::Active::_on_reservation_node_allocate_waitpoint( + const rmf_traffic::agv::Plan::Goal& goal) +{ + RCLCPP_INFO(_context->node()->get_logger(), + "Received waitpoint from reservation node"); + _chosen_goal = goal; + _is_final_destination = false; + _find_plan(); +} + //============================================================================== rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const { @@ -357,6 +429,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()) + { + _reservation_client->cancel(); + } _finished(); } @@ -366,46 +443,20 @@ void GoToPlace::Active::kill() _stop_and_clear(); _state->update_status(Status::Killed); _state->update_log().info("Received signal to 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) + if (_context->_parking_spot_manager_enabled()) { - const auto& wp = g.get_waypoint(l.waypoint()); - if (wp.name()) - return *wp.name(); + _reservation_client->cancel(); } - - if (locations.empty()) - return ""; - - return "#" + std::to_string(locations.front().waypoint()); + _finished(); } + //============================================================================== std::optional GoToPlace::Active::_choose_goal( 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. @@ -422,60 +473,70 @@ std::optional GoToPlace::Active::_choose_goal( _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) + + if (_context->_parking_spot_manager_enabled()) + { + RCLCPP_INFO(_context->node()->get_logger(), + "Waiting for next location from reservation node."); + } + else { - const auto wp_idx = _description.one_of()[i].waypoint(); - if (only_same_map) + 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()) + // 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(), - "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) + if (result->cost() < lowest_cost) + { + selected_idx = i; + lowest_cost = result->cost(); + } + } + else { - selected_idx = i; - lowest_cost = result->cost(); + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + _context->requester_id().c_str(), + wp_idx); } } - else + + if (selected_idx.has_value()) { - RCLCPP_ERROR( - _context->node()->get_logger(), - "No path found for robot [%s] to waypoint [%lu]", - _context->requester_id().c_str(), - wp_idx); + return _description.one_of()[*selected_idx]; } } - if (selected_idx.has_value()) - { - return _description.one_of()[*selected_idx]; - } - return std::nullopt; } @@ -485,7 +546,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); } @@ -512,6 +573,17 @@ void GoToPlace::Active::_find_plan() return; } + if (_context->location().size() == 0) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] is lost while trying to find a plan.", + _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); @@ -624,7 +696,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; @@ -657,24 +731,44 @@ 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; } 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()); - _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) + { + 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, [&]() + { + _stop_and_clear(); + _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()) { @@ -693,6 +787,8 @@ void GoToPlace::Active::_stop_and_clear() if (const auto command = _context->command()) command->stop(); + if (_retry_timer) + _retry_timer->cancel(); _context->itinerary().clear(); } @@ -707,7 +803,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) @@ -715,8 +811,12 @@ 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 fca09590..3b64592e 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,10 @@ #include "../services/FindPath.hpp" #include "ExecutePlan.hpp" +#include "internal_ReservationNodeNegotiator.hpp" +#include +#include #include #include #include @@ -104,10 +107,12 @@ class GoToPlace : public rmf_task_sequence::Event private: + 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) const; @@ -121,6 +126,12 @@ class GoToPlace : public rmf_task_sequence::Event void _stop_and_clear(); + void _on_reservation_node_allocate_final_destination( + const rmf_traffic::agv::Plan::Goal& goal); + + void _on_reservation_node_allocate_waitpoint( + const rmf_traffic::agv::Plan::Goal& goal); + Negotiator::NegotiatePtr _respond( const Negotiator::TableViewerPtr& table_view, const Negotiator::ResponderPtr& responder); @@ -143,7 +154,11 @@ 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; + 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_ReservationNodeNegotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp new file mode 100644 index 00000000..467fe0b8 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/internal_ReservationNodeNegotiator.hpp @@ -0,0 +1,349 @@ +/* + * 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_RESERVATION_NEGOTIATOR_HPP +#define SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_RESERVATION_NEGOTIATOR_HPP + +#include "../agv/RobotContext.hpp" +#include "internal_utilities.hpp" +#include + +namespace rmf_fleet_adapter { +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 +{ +public: + ReservationNodeNegotiator( + std::shared_ptr context, + const std::vector goals, + const std::function + selected_final_destination_cb, + const std::function selected_waitpoint_cb) + { + _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(); + } + + void cancel() + { + _context->worker().schedule( + [ptr = weak_from_this()]( + const auto&) + { + auto self = ptr.lock(); + if(!self) + { + return; + } + + self->_context->_cancel_allocated_destination(); + }); + } + + 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, 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_DEBUG( + self->_context->node()->get_logger(), + "Reservations: 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; + self->_waitpoints = self->_context->_find_and_sort_parking_spots( + true); + + if (self->_waitpoints.size() == 0) + { + RCLCPP_ERROR( + self->_context->node()->get_logger(), + "Reservations: Got no waitpoints for %s", self->_context->requester_id().c_str()); + return; + } + + // Immediately make claim cause we don't yet support flexible reservations. + rmf_reservation_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_DEBUG( + self->_context->node()->get_logger(), + "Reservations: Claim issued by %s", self->_context->name().c_str()); + }); + + + 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; + } + + if (msg->ticket.ticket_id != self->_ticket.value()->ticket_id) + { + return; + } + + self->_final_allocated_destination = msg; + self->_context->_set_allocated_destination(*msg.get()); + + if (msg->instruction_type + == rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED) + { + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Reservation: Robot %s is going to final destination %lu", + self->_context->name().c_str(), + self->_goals[self->_final_allocated_destination.value()-> + chosen_alternative].waypoint()); + self->_current_reservation_state = ReservationState::ReceivedResponseProceedImmediate; + self->_selected_final_destination_cb(self->_goals[self-> + _final_allocated_destination.value()-> + chosen_alternative].waypoint()); + } + + if (msg->instruction_type + == rmf_reservation_msgs::msg::ReservationAllocation::WAIT_IDENTIFIED) + { + self->_current_reservation_state = ReservationState::ReceivedResponseProceedWaitPoint; + self->_selected_waitpoint_cb(self->_waitpoints[self-> + _final_allocated_destination.value()-> + 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()-> + chosen_alternative].waypoint()); + } + }); + + + 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(), same_map]( + const auto&) + { + auto negotiator = ptr.lock(); + if (!negotiator) + return; + + negotiator->make_request(same_map); + }); + return negotiator; + } + +private: + + enum class ReservationState + { + Pending=0, + Requested=1, + ReceivedResponseProceedWaitPoint=2, + ReceivedResponseProceedImmediate=3 + }; + + + void make_request(bool only_same_map) + { + auto current_location = _context->location(); + const auto& graph = _context->navigation_graph(); + if (current_location.size() == 0) + { + //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", + _context->requester_id().c_str()); + return; + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "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_reservation_msgs::msg::FlexibleTimeRequest ftr; + ftr.header.robot_name = _context->name(); + ftr.header.fleet_name = _context->group(); + ftr.header.request_id = _reservation_id; + + std::optional selected_idx; + 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) + { + + // 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()); + + 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_reservation_msgs::msg::FlexibleTimeReservationAlt alternative; + alternative.resource_name = *name; + alternative.cost = result->cost(); + alternative.has_end = false; + + 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; + 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 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 00000000..6929a6b8 --- /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()); +} + +} +} 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 00000000..ea5420bb --- /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 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 be9c81c6..9139d20c 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 2c03adb6..2c99aabd 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 diff --git a/rmf_reservation_node/CMakeLists.txt b/rmf_reservation_node/CMakeLists.txt new file mode 100644 index 00000000..bc7667f4 --- /dev/null +++ b/rmf_reservation_node/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.8) +project(rmf_reservation_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_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_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}) + +install(TARGETS + queue_manager + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/rmf_reservation_node/README.md b/rmf_reservation_node/README.md new file mode 100644 index 00000000..6689e67d --- /dev/null +++ b/rmf_reservation_node/README.md @@ -0,0 +1,54 @@ +# rmf\_reservation\_node package + +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`. + + +## 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 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 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 tinyRobot -R 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 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 tinyRobot -R tinyRobot2 --use_sim_time +``` +`tinyRobot1` and `tinyRobot2` should now proceed to swap places. + +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 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 +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 + +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_reservation_node/package.xml b/rmf_reservation_node/package.xml new file mode 100644 index 00000000..90597d79 --- /dev/null +++ b/rmf_reservation_node/package.xml @@ -0,0 +1,22 @@ + + + + rmf_reservation_node + 0.0.0 + Node that handles current state of parking spots. + Arjo Chakravarty + Apache License 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rmf_building_map_msgs + rmf_reservation_msgs + rmf_fleet_adapter + + + ament_cmake + + diff --git a/rmf_reservation_node/src/main.cpp b/rmf_reservation_node/src/main.cpp new file mode 100644 index 00000000..0d542943 --- /dev/null +++ b/rmf_reservation_node/src/main.cpp @@ -0,0 +1,641 @@ +/* + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace rmf_fleet_adapter; +/// C++-isms +template<> +struct std::hash +{ + std::size_t operator()(const rmf_reservation_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); + } +}; + + +/// Ticket generation class for book keeping purposes. Will eventually overflow. +/// Ticket ids are mapped across multiple fleets. +class TicketStore +{ + +public: + rmf_reservation_msgs::msg::Ticket get_new_ticket( + const rmf_reservation_msgs::msg::RequestHeader& request_header) + { + rmf_reservation_msgs::msg::Ticket ticket; + ticket.header = request_header; + ticket.ticket_id = _next_ticket_id; + + _ticket_to_header.emplace(_next_ticket_id, request_header); + _next_ticket_id++; + return ticket; + } + + rmf_reservation_msgs::msg::Ticket get_existing_ticket(const std::size_t index) + { + rmf_reservation_msgs::msg::Ticket ticket; + ticket.ticket_id = 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 _next_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. +class CurrentState +{ +public: + /// Get list of free locations + std::vector free_locations() const + { + std::vector locations; + for (auto&[loc, state] : _current_location_reservations) + { + if (!state.ticket.has_value()) + { + locations.push_back(loc); + } + } + return locations; + } + + void add_location(std::shared_ptr node, std::string location) + { + if (location.empty()) + { + 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) + { + _current_location_reservations.emplace(location, + LocationState {std::nullopt}); + } + else + { + + RCLCPP_ERROR(node->get_logger(), + "Got duplicate location: [%s]", location.c_str()); + } + } + + /// 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. + std::optional allocate_lowest_cost_free_spot( + std::shared_ptr node, + std::vector requests, + const std::size_t ticket_id) + { + if (_ticket_to_location.count(ticket_id) != 0) + { + /// Release previous instance and reallocate ticket + release(ticket_id); + } + + std::unordered_map positions; + for (std::size_t i = 0; i < requests.size(); ++i) + { + positions[requests[i].location] = i; + } + + 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()) + { + // 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 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 positions[parking->first]; + } + } + + std::stringstream str; + str << "Could not find a free space from any of: "; + for (auto c: requests) + { + str << c.location << ", "; + } + RCLCPP_ERROR(node->get_logger(), + "%s", str.str().c_str()); + return std::nullopt; + } + + std::optional release(const std::size_t ticket_id) + { + auto _ticket = _ticket_to_location.find(ticket_id); + if (_ticket == _ticket_to_location.end()) + { + return std::nullopt; + } + auto location = _ticket->second; + _current_location_reservations[location].ticket = std::nullopt; + _ticket_to_location.erase(_ticket); + return {location}; + } + +private: + std::unordered_map _current_location_reservations; + std::unordered_map _ticket_to_location; +}; + + +/// A queue that allows 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) + { + // 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); + curr_index++; + } + + // Removes the item from the queue + // Log(n) + void remove_item(T item) + { + auto index = item_to_index.find(item); + if (index == item_to_index.end()) + { + return; + } + auto idx = index->second; + item_to_index.erase(item); + index_to_item.erase(idx); + indices.erase(idx); + } + + // Gives the most recent item in the queue. + // Returns nullopt if the queue is empty. + 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. +/// The basic idea is that we maintain a queue for every resource. As requests +/// 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_map> resource_queues; +public: + /// Service + std::optional service_next_in_queue(const std::string& resource) + { + auto item = resource_queues[resource].front(); + if (!item.has_value()) + { + return std::nullopt; + } + + remove_ticket(item.value()); + return item; + } + + void add_to_queue( + std::size_t ticket, std::vector& resources) + { + for (auto resource: resources) + { + 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; + +class ReservationNode : public rclcpp::Node +{ +public: + ReservationNode() + : Node("rmf_reservation_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(&ReservationNode::on_request, this, + std::placeholders::_1)); + claim_subscription_ = + this->create_subscription( + 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)); + cancel_subscription_ = + this->create_subscription( + ReservationCancelTopicName, qos, + std::bind(&ReservationNode::cancel, this, std::placeholders::_1)); + graph_subscription_ = + this->create_subscription( + NavGraphTopicName, qos, + std::bind(&ReservationNode::received_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(&ReservationNode::publish_free_spots, this)); + } + +private: + void received_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& param: graph_msg->vertices[i].params) + { + + //TODO(arjoc) make this configure-able + if (param.name == "is_parking_spot" && param.value_bool) + { + current_state_.add_location(shared_from_this(), + graph_msg->vertices[i].name); + } + } + } + } + void on_request( + const rmf_reservation_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_reservation_msgs::msg::ClaimRequest::ConstSharedPtr& request) + { + + // This logic is for the simplified queue-less version. + std::vector 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 + auto result = current_state_.allocate_lowest_cost_free_spot( + shared_from_this(), + locations, + request->ticket.ticket_id); + if (result.has_value()) + { + rmf_reservation_msgs::msg::ReservationAllocation allocation; + allocation.ticket = ticket_store_.get_existing_ticket( + request->ticket.ticket_id); + allocation.instruction_type = + rmf_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + allocation.chosen_alternative = result.value(); + allocation.resource = + requests_[request->ticket.ticket_id][result.value()].location; + + 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); + return; + } + + // If we can't proceed immediately add the ticket to a queue. + 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++) + { + 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 + 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( + shared_from_this(), + wait_points, + request->ticket.ticket_id); + if (waitpoint_result.has_value()) + { + rmf_reservation_msgs::msg::ReservationAllocation allocation; + allocation.ticket = ticket_store_.get_existing_ticket( + request->ticket.ticket_id); + allocation.instruction_type = + 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(), + 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()); + } + } + + 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) + { + 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; + 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.", + request->ticket.ticket_id); + return; + } + RCLCPP_DEBUG( + this->get_logger(), "Released ticket %lu, location %s is now free", + request->ticket.ticket_id, + previous_waiting_location->c_str()); + + // Traverse waitgraph. + while (auto next_ticket = + queue_manager_.service_next_in_queue(previous_waiting_location.value())) + { + // Release the ticket + previous_waiting_location = current_state_.release(next_ticket.value()); + if (!previous_waiting_location.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), + "Could not find ticket %lu while traversing wait graph. Something has gone wrong.", + next_ticket.value()); + return; + } + + auto result = + 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", + next_ticket.value(), + previous_waiting_location.value().c_str()); + + + if (!result.has_value()) + { + RCLCPP_ERROR( + this->get_logger(), + "Tried to service %lu. Apparently there was some inconsistency" + " between the reservation node's state and the fleetadapter.", + ticket); + return; + } + rmf_reservation_msgs::msg::ReservationAllocation allocation; + allocation.chosen_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_reservation_msgs::msg::ReservationAllocation::IMMEDIATELY_PROCEED; + 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(), + ticket_store_.debug_ticket(request->ticket.ticket_id).c_str()); + allocation_pub_->publish(allocation); + } + RCLCPP_DEBUG( + this->get_logger(), "Queue is now empty %s", + previous_waiting_location->c_str()); + return; + } + + + void publish_free_spots() + { + rmf_reservation_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 + cancel_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_; + std::unordered_map> waitpoints_; + TicketStore ticket_store_; + CurrentState current_state_; + ServiceQueueManager queue_manager_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +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_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index baa12325..25f86c9e 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -28,6 +28,8 @@ find_package(yaml-cpp REQUIRED) find_package(nlohmann_json REQUIRED) find_package(ZLIB REQUIRED) find_package(LibUUID REQUIRED) +find_package(rmf_reservation_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. @@ -182,6 +184,7 @@ target_link_libraries(rmf_traffic_ros2 ${rmf_traffic_msgs_LIBRARIES} ${rmf_site_map_msgs_LIBRARIES} ${rmf_building_map_msgs_LIBRARIES} + ${rmf_reservation_msgs_LIBRARIES} ${rclcpp_LIBRARIES} yaml-cpp ZLIB::ZLIB @@ -197,6 +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_reservation_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index af48d9f4..30ce29df 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -16,6 +16,7 @@ proj rclcpp rmf_building_map_msgs + rmf_reservation_msgs rmf_fleet_msgs rmf_site_map_msgs rmf_traffic_msgs