From cbb6f514807c9c3cc639c852cf8e886723c692fd Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 5 Sep 2023 22:12:03 +0800 Subject: [PATCH 01/89] Funnel emergency notifications through the fleet adapter Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/TaskManager.cpp | 7 ++++++- .../agv/FleetUpdateHandle.cpp | 19 +++++++++++++++++++ .../agv/internal_FleetUpdateHandle.hpp | 17 +++++++++++++++++ 3 files changed, 42 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 430cacfdf..24bcc69ff 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) { + auto fleet = fleet_handle.lock(); + if (!fleet) + return nullptr; + auto mgr = TaskManagerPtr( new TaskManager( std::move(context), @@ -115,7 +119,8 @@ 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) 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..b52be7490 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1016,6 +1016,25 @@ 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) + { + + } + else + { + + } + + emergency_publisher.get_subscriber().on_next(is_emergency); +} + //============================================================================== nlohmann::json_schema::json_validator FleetUpdateHandle::Implementation::make_validator( 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..d5d5dc18a 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 @@ -277,6 +277,11 @@ class FleetUpdateHandle::Implementation rclcpp::TimerBase::SharedPtr fleet_state_update_timer = nullptr; rclcpp::TimerBase::SharedPtr memory_trim_timer = nullptr; + rxcpp::subscription emergency_sub; + rxcpp::subjects::subject emergency_publisher; + rxcpp::observable emergency_obs; + bool emergency_active = false; + // Map task id to pair of using Assignments = rmf_task::TaskPlanner::Assignments; @@ -334,6 +339,17 @@ class FleetUpdateHandle::Implementation handle->_pimpl->add_standard_tasks(); + 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); + } + }); + // 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 @@ -590,6 +606,7 @@ class FleetUpdateHandle::Implementation void update_fleet_state() const; void update_fleet_logs() const; + void handle_emergency(bool is_emergency); nlohmann::json_schema::json_validator make_validator( const nlohmann::json& schema) const; From b917e10a75ad4b774ffccbdc379287ac936c4a6e Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 6 Sep 2023 19:49:22 +0800 Subject: [PATCH 02/89] Fix compilation Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 24bcc69ff..2355ee314 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -123,15 +123,15 @@ TaskManagerPtr TaskManager::make( .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) { From a8fb88e6fc83f1ddf138d1d145517b83ab083ce9 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 6 Sep 2023 22:12:44 +0800 Subject: [PATCH 03/89] Implement emergency lift closing Signed-off-by: Michael X. Grey --- .../agv/FleetUpdateHandle.cpp | 124 +++++++++++++++--- .../rmf_fleet_adapter/agv/RobotContext.cpp | 11 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 15 ++- .../agv/internal_FleetUpdateHandle.hpp | 9 +- .../events/EmergencyPullover.cpp | 5 +- 5 files changed, 141 insertions(+), 23 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 b52be7490..3f9db02e6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1025,14 +1025,96 @@ void FleetUpdateHandle::Implementation::handle_emergency(const bool is_emergency emergency_active = is_emergency; if (is_emergency) { + update_emergency_planner(); + } + + 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; } - else + 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); + lane.entry().event()->execute(executor); + lane.exit().event()->execute(executor); + 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); + } + } + } } - emergency_publisher.get_subscriber().on_next(is_emergency); + 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)); } //============================================================================== @@ -1382,6 +1464,7 @@ void FleetUpdateHandle::add_robot( std::move(participant), fleet->_pimpl->mirror, fleet->_pimpl->planner, + fleet->_pimpl->emergency_planner, fleet->_pimpl->activation.task, fleet->_pimpl->task_parameters, fleet->_pimpl->node, @@ -1597,16 +1680,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; } } @@ -1616,19 +1695,25 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) return; } + const auto& current_lane_closures = + (*self->_pimpl->planner)->get_configuration().lane_closures(); + auto new_config = (*self->_pimpl->planner)->get_configuration(); auto& new_lane_closures = new_config.lane_closures(); 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(); @@ -1650,16 +1735,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; } } @@ -1669,19 +1755,25 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) return; } + const auto& current_lane_closures = + (*self->_pimpl->planner)->get_configuration().lane_closures(); + auto new_config = (*self->_pimpl->planner)->get_configuration(); auto& new_lane_closures = new_config.lane_closures(); 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(); }); 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..2a8da38c9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -235,6 +235,13 @@ RobotContext::planner() const return *_planner; } +//============================================================================== +const std::shared_ptr& +RobotContext::emergency_planner() const +{ + return *_emergency_planner; +} + //============================================================================== std::shared_ptr RobotContext::nav_params() const { @@ -606,7 +613,8 @@ RobotContext::RobotContext( 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 +627,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)), 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..44c568d5b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -50,6 +50,8 @@ class TaskManager; namespace agv { using TransformDictionary = std::unordered_map; +using SharedPlanner = std::shared_ptr< + std::shared_ptr>; //============================================================================== struct NavParams @@ -201,9 +203,14 @@ class RobotContext /// 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; @@ -365,7 +372,8 @@ class RobotContext 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, @@ -381,7 +389,8 @@ class RobotContext 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; 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 d5d5dc18a..2be235a2c 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 @@ -239,7 +239,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; @@ -281,6 +281,10 @@ class FleetUpdateHandle::Implementation 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; // Map task id to pair of using Assignments = rmf_task::TaskPlanner::Assignments; @@ -349,6 +353,8 @@ class FleetUpdateHandle::Implementation 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 @@ -607,6 +613,7 @@ class FleetUpdateHandle::Implementation void update_fleet_state() const; void update_fleet_logs() const; void handle_emergency(bool is_emergency); + void update_emergency_planner(); nlohmann::json_schema::json_validator make_validator( const nlohmann::json& schema) 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 d64f29ef2..6b4f47287 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 = @@ -371,7 +372,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)); } From 0768ade5733471559fa92c5b6dc9d64b9e83f9c0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 7 Sep 2023 14:51:27 +0800 Subject: [PATCH 04/89] Add API and config file parsing for setting lift emergency level Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 32 +++++++++++ .../agv/FleetUpdateHandle.hpp | 20 +++++++ .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 55 ++++++++++++++++++- .../agv/FleetUpdateHandle.cpp | 9 +++ rmf_fleet_adapter_python/src/adapter.cpp | 8 ++- 5 files changed, 121 insertions(+), 3 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 b5f9531c9..fc9d680e4 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -743,6 +743,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/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index a60f57043..87f6c8fdc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1559,6 +1559,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 +1613,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 @@ -2126,7 +2128,31 @@ EasyFullControl::FleetConfiguration::from_config_files( } } - 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 +2176,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 +2523,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; 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 3f9db02e6..9e6375b0e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1779,6 +1779,15 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) }); } +//============================================================================== +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 { diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index ac83f9a68..79da951db 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -1031,7 +1031,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") From 3ab538cab2a4f409a7aff7620af7147e5c5587f0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 12 Sep 2023 14:27:21 +0800 Subject: [PATCH 05/89] Fixed and tested fire alarm lift response Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/TaskManager.cpp | 2 +- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 5 ++ .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 35 +++++++----- .../agv/FleetUpdateHandle.cpp | 24 ++++++--- .../rmf_fleet_adapter/agv/RobotContext.cpp | 54 ++++++++++++------- .../rmf_fleet_adapter/agv/RobotContext.hpp | 8 +++ .../agv/internal_FleetUpdateHandle.hpp | 1 + 7 files changed, 90 insertions(+), 39 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 2355ee314..f432cc6b1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -73,7 +73,7 @@ TaskManagerPtr TaskManager::make( std::optional> broadcast_client, std::weak_ptr fleet_handle) { - auto fleet = fleet_handle.lock(); + std::shared_ptr fleet = fleet_handle.lock(); if (!fleet) return nullptr; 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..c95e17123 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -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 87f6c8fdc..75d937827 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -297,7 +297,7 @@ class EasyFullControl::CommandExecution::Implementation } const auto& graph = planner->get_configuration().graph(); - const auto& closed_lanes = planner->get_configuration().lane_closures(); + 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]; @@ -350,7 +350,7 @@ class EasyFullControl::CommandExecution::Implementation return; } - if (closed_lanes.is_closed(lane_id)) + if (closures && closures->is_closed(lane_id)) { // Don't use a lane that's closed continue; @@ -379,7 +379,7 @@ class EasyFullControl::CommandExecution::Implementation return; } - if (closed_lanes.is_closed(lane_id)) + if (closures && closures->is_closed(lane_id)) { continue; } @@ -390,14 +390,24 @@ class EasyFullControl::CommandExecution::Implementation 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) + double dist_to_lane = 0.0; + if (lane_length < nav_params->min_lane_length) { - continue; + dist_to_lane = std::min( + (p - p0).norm(), + (p - p1).norm()); + } + else + { + const auto lane_u = (p1 - p0)/lane_length; + const auto proj = (p - p0).dot(lane_u); + if (proj < 0.0 || lane_length < proj) + { + continue; + } + dist_to_lane = (p - p0 - proj * lane_u).norm(); } - 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) @@ -422,10 +432,11 @@ class EasyFullControl::CommandExecution::Implementation now, wp0, yaw, p, reverse_lane->index())); } } - else - { - starts = nav_params->compute_plan_starts(graph, map, location, now); - } + } + + if (starts.empty()) + { + starts = nav_params->compute_plan_starts(graph, map, location, now); } if (!starts.empty()) 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 9e6375b0e..9f378e6a7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1028,6 +1028,10 @@ void FleetUpdateHandle::Implementation::handle_emergency(const bool is_emergency update_emergency_planner(); } + for (const auto& [context, _] : task_managers) + { + context->_set_emergency(is_emergency); + } emergency_publisher.get_subscriber().on_next(is_emergency); } @@ -1069,8 +1073,14 @@ std::vector find_emergency_lift_closures( { EmergencyLaneCloser executor; const auto& lane = graph.get_lane(i); - lane.entry().event()->execute(executor); - lane.exit().event()->execute(executor); + 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) @@ -1491,6 +1501,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 @@ -1695,9 +1709,6 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) return; } - const auto& current_lane_closures = - (*self->_pimpl->planner)->get_configuration().lane_closures(); - auto new_config = (*self->_pimpl->planner)->get_configuration(); auto& new_lane_closures = new_config.lane_closures(); for (const auto& lane : lane_indices) @@ -1755,9 +1766,6 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) return; } - const auto& current_lane_closures = - (*self->_pimpl->planner)->get_configuration().lane_closures(); - auto new_config = (*self->_pimpl->planner)->get_configuration(); auto& new_lane_closures = new_config.lane_closures(); for (const auto& lane : lane_indices) 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 2a8da38c9..fbfbf677c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -142,30 +142,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 +160,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() { @@ -669,5 +677,15 @@ 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(); + } +} + } // 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 44c568d5b..c83f1de57 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -174,6 +174,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(); @@ -367,6 +370,10 @@ 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); + RobotContext( std::shared_ptr command_handle, std::vector _initial_location, @@ -425,6 +432,7 @@ class RobotContext RobotUpdateHandle::Unstable::Watchdog _lift_watchdog; rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0); bool _commissioned = true; + bool _emergency = false; // Mode value for RobotMode message uint32_t _current_mode; 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 2be235a2c..dc09596b1 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 @@ -343,6 +343,7 @@ 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( From 2810de528612a947b89803483ee1937b8238d490 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Sep 2023 06:13:56 +0000 Subject: [PATCH 06/89] Provide an explicit hook for localization Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 21 ++++- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 92 +++++++++++-------- .../rmf_fleet_adapter/agv/RobotContext.cpp | 21 +++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 25 +++++ .../agv/internal_EasyFullControl.hpp | 20 ++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 39 +++++--- .../rmf_fleet_adapter/phases/RequestLift.cpp | 28 ++++-- .../rmf_fleet_adapter/phases/RequestLift.hpp | 11 ++- 8 files changed, 192 insertions(+), 65 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 fc9d680e4..67303056c 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,14 @@ 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. + using LocalizationRequest = std::function; + /// Add a robot to the fleet once it is available. /// /// \param[in] name @@ -333,6 +345,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; 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 75d937827..2df88971e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -217,6 +217,7 @@ class EasyFullControl::RobotCallbacks::Implementation NavigationRequest navigate; StopRequest stop; ActionExecutor action_executor; + LocalizationRequest localize; }; //============================================================================== @@ -227,7 +228,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 @@ -251,6 +253,20 @@ auto EasyFullControl::RobotCallbacks::action_executor() const -> ActionExecutor return _pimpl->action_executor; } +//============================================================================== +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 { @@ -642,26 +658,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 { @@ -805,28 +801,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; @@ -2587,6 +2577,29 @@ 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) + { + auto robot_position = nav_params->to_robot_coordinates( + estimate.map(), + estimate.position()); + if (robot_position.has_value()) + { + auto robot_estimate = Destination::Implementation::make( + estimate.map(), + robot_position, + estimate.graph_index(), + estimate.speed_limit()); + inner(robot_estimate); + } + }; + } + const auto& fleet_impl = FleetUpdateHandle::Implementation::get(*_pimpl->fleet_handle); const auto& planner = *fleet_impl.planner; @@ -2741,6 +2754,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) @@ -2758,6 +2772,7 @@ auto EasyFullControl::add_robot( fleet_name, charger_index, action_executor, + localization, context, nav_params, enable_responsive_wait @@ -2768,6 +2783,7 @@ auto EasyFullControl::add_robot( EasyRobotUpdateHandle::Implementation::get(*easy_updater) .updater->handle = handle; handle->set_action_executor(action_executor); + context->set_localization(localization); handle->set_charger_waypoint(charger_index); handle->enable_responsive_wait(enable_responsive_wait); 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 fbfbf677c..faadb5887 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -23,6 +23,8 @@ #include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -83,6 +85,11 @@ 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()) @@ -615,6 +622,20 @@ const Reporting& RobotContext::reporting() const return _reporting; } +//============================================================================== +void RobotContext::localize(EasyFullControl::Destination estimate) const +{ + if (_localize) + _localize(std::move(estimate)); +} + +//============================================================================== +void RobotContext::set_localization( + EasyFullControl::LocalizationRequest localization) +{ + _localize = std::move(localization); +} + //============================================================================== RobotContext::RobotContext( std::shared_ptr command_handle, 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 c83f1de57..cb5fcb188 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 @@ -52,6 +53,7 @@ namespace agv { using TransformDictionary = std::unordered_map; using SharedPlanner = std::shared_ptr< std::shared_ptr>; +using Destination = EasyFullControl::Destination; //============================================================================== struct NavParams @@ -83,6 +85,22 @@ 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); + } + rmf_traffic::agv::Plan::StartSet compute_plan_starts( const rmf_traffic::agv::Graph& graph, const std::string& map_name, @@ -362,6 +380,12 @@ class RobotContext const Reporting& reporting() const; + /// Tell the robot to localize near here + void localize(EasyFullControl::Destination estimate) const; + + /// Set the callback for localizing the robot + void set_localization(EasyFullControl::LocalizationRequest localization); + /// 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); @@ -433,6 +457,7 @@ class RobotContext 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; 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..ebcb68a3c 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 @@ -67,5 +67,25 @@ class EasyFullControl::Implementation } }; +//============================================================================== +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; + } +}; + } // namespace agv } // namespace rmf_fleet_adapter 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..c022674fe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -26,6 +26,8 @@ #include "../phases/RequestLift.hpp" #include "../phases/DockRobot.hpp" +#include "../agv/internal_EasyFullControl.hpp" + #include namespace rmf_fleet_adapter { @@ -90,25 +92,26 @@ 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& waypoint_, bool& continuous) - : _context(std::move(context)), + : waypoint(waypoint_), + _context(std::move(context)), _phases(phases), - _event_start_time(event_start_time), - _dependencies(dependencies), + _event_start_time(waypoint_.time()), _continuous(continuous) { // Do nothing } + rmf_traffic::agv::Plan::Waypoint waypoint; + void execute(const Dock& dock) final { assert(!_moving_lift); _phases.emplace_back( std::make_shared( _context, dock.dock_name()), - _event_start_time, _dependencies); + _event_start_time, waypoint.dependencies()); _continuous = false; } @@ -122,7 +125,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, waypoint.dependencies()); _continuous = true; } @@ -137,7 +140,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, _dependencies); + _event_start_time, waypoint.dependencies()); _continuous = true; } @@ -152,7 +155,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.floor_name(), _event_start_time, phases::RequestLift::Located::Outside), - _event_start_time, _dependencies); + _event_start_time, waypoint.dependencies()); _continuous = true; } @@ -170,6 +173,12 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftDoorOpen& open) final { const auto node = _context->node(); + auto localize = agv::Destination::Implementation::make( + open.floor_name(), + waypoint.position(), + waypoint.graph_index(), + std::nullopt, + std::nullopt); // TODO(MXG): The time calculation here should be considered more carefully. _phases.emplace_back( @@ -178,8 +187,9 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.lift_name(), open.floor_name(), _event_start_time + open.duration() + _lifting_duration, - phases::RequestLift::Located::Inside), - _event_start_time, _dependencies); + phases::RequestLift::Located::Inside, + localize), + _event_start_time, waypoint.dependencies()); _moving_lift = false; _continuous = true; @@ -194,7 +204,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, _dependencies); + _event_start_time, waypoint.dependencies()); _continuous = true; } @@ -213,7 +223,6 @@ 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; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); @@ -474,8 +483,7 @@ std::optional ExecutePlan::make( move_through.clear(); bool continuous = true; - EventPhaseFactory factory( - context, legacy_phases, it->time(), it->dependencies(), continuous); + EventPhaseFactory factory(context, legacy_phases, *it, continuous); it->event()->execute(factory); while (factory.moving_lift()) { @@ -499,6 +507,7 @@ std::optional ExecutePlan::make( "navigation graph. Please report this to the system integrator."); } + factory.waypoint = *it; it->event()->execute(factory); } 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..61a4fd2c0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -28,7 +28,8 @@ std::shared_ptr RequestLift::ActivePhase::make( std::string lift_name, std::string destination, rmf_traffic::Time expected_finish, - const Located located) + const Located located, + std::optional localize) { auto inst = std::shared_ptr( new ActivePhase( @@ -36,7 +37,8 @@ std::shared_ptr RequestLift::ActivePhase::make( std::move(lift_name), std::move(destination), std::move(expected_finish), - located + located, + std::move(localize) )); inst->_init_obs(); return inst; @@ -80,12 +82,14 @@ RequestLift::ActivePhase::ActivePhase( std::string lift_name, std::string destination, rmf_traffic::Time expected_finish, - Located located) + Located located, + std::optional localize) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), _expected_finish(std::move(expected_finish)), - _located(located) + _located(located), + _localize_after(std::move(localize)) { std::ostringstream oss; oss << "Requesting lift [" << lift_name << "] to [" << destination << "]"; @@ -99,6 +103,7 @@ void RequestLift::ActivePhase::_init_obs() using rmf_lift_msgs::msg::LiftState; _obs = _context->node()->lift_state() + .observe_on(rxcpp::identity_same_worker(_context->worker())) .lift( on_subscribe( [weak = weak_from_this()]() @@ -166,7 +171,11 @@ void RequestLift::ActivePhase::_init_obs() if (!me) return; - // FIXME: is this thread-safe? + if (me->_localize_after.has_value()) + { + me->_context->localize(*me->_localize_after); + } + if (!me->_cancelled.get_value() || me->_located == Located::Inside) { s.on_completed(); @@ -320,12 +329,14 @@ RequestLift::PendingPhase::PendingPhase( std::string lift_name, std::string destination, rmf_traffic::Time expected_finish, - Located located) + Located located, + std::optional localize) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), _expected_finish(std::move(expected_finish)), - _located(located) + _located(located), + _localize_after(std::move(localize)) { std::ostringstream oss; oss << "Requesting lift \"" << lift_name << "\" to \"" << destination << "\""; @@ -341,7 +352,8 @@ std::shared_ptr RequestLift::PendingPhase::begin() _lift_name, _destination, _expected_finish, - _located); + _located, + _localize_after); } //============================================================================== 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..6154080aa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -44,7 +44,8 @@ struct RequestLift std::string lift_name, std::string destination, rmf_traffic::Time expected_finish, - Located located); + Located located, + std::optional localize); const rxcpp::observable& observe() const override; @@ -69,6 +70,7 @@ struct RequestLift rclcpp::TimerBase::SharedPtr _timer; std::shared_ptr _lift_end_phase; Located _located; + std::optional _localize_after; rmf_rxcpp::subscription_guard _reset_session_subscription; struct WatchdogInfo @@ -86,7 +88,8 @@ struct RequestLift std::string lift_name, std::string destination, rmf_traffic::Time expected_finish, - Located located); + Located located, + std::optional localize); void _init_obs(); @@ -105,7 +108,8 @@ struct RequestLift std::string lift_name, std::string destination, rmf_traffic::Time expected_finish, - Located located); + Located located, + std::optional localize = std::nullopt); std::shared_ptr begin() override; @@ -124,6 +128,7 @@ struct RequestLift std::string _destination; rmf_traffic::Time _expected_finish; Located _located; + std::optional _localize_after; std::string _description; }; }; From 6560b44efc076f40f4fd2d69d0cb20dbcad283ce Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 20 Sep 2023 06:26:32 +0000 Subject: [PATCH 07/89] Fix syntax and add python bindings Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 4 ++-- rmf_fleet_adapter_python/src/adapter.cpp | 7 ++++++- 2 files changed, 8 insertions(+), 3 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 2df88971e..07cb8c667 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -2582,7 +2582,7 @@ auto EasyFullControl::add_robot( { localization = [ inner = callbacks.localize(), - nav_params = robot_nav_params, + nav_params = robot_nav_params ](Destination estimate) { auto robot_position = nav_params->to_robot_coordinates( @@ -2592,7 +2592,7 @@ auto EasyFullControl::add_robot( { auto robot_estimate = Destination::Implementation::make( estimate.map(), - robot_position, + *robot_position, estimate.graph_index(), estimate.speed_limit()); inner(robot_estimate); diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 79da951db..d6b1b5afb 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -812,7 +812,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) From 11603232797341710caea43a1647a8cdb7a86e57 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 21 Sep 2023 13:19:58 +0000 Subject: [PATCH 08/89] Use CommandExecution with LocalizationRequest Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 7 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 581 +++++++++--------- .../rmf_fleet_adapter/agv/RobotContext.cpp | 39 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 10 +- .../agv/internal_EasyFullControl.hpp | 38 ++ .../agv/internal_RobotUpdateHandle.hpp | 1 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 10 +- .../rmf_fleet_adapter/phases/RequestLift.cpp | 60 +- .../rmf_fleet_adapter/phases/RequestLift.hpp | 5 + .../test/phases/test_RequestLift.cpp | 3 +- 10 files changed, 447 insertions(+), 307 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 67303056c..4f227c8f3 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -96,8 +96,13 @@ class EasyFullControl : public std::enable_shared_from_this /// /// \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; + Destination location_estimate, + CommandExecution execution)>; /// Add a robot to the fleet once it is available. /// 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 07cb8c667..01ec4e022 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -268,345 +268,363 @@ auto EasyFullControl::RobotCallbacks::localize() const -> LocalizationRequest } //============================================================================== -class EasyFullControl::CommandExecution::Implementation +class EasyFullControl::CommandExecution::Implementation::Data { public: - struct Data + 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; + + 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 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) + 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 (closures && closures->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 (closures && closures->is_closed(lane_id)) + 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& 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 - { - const auto lane_u = (p1 - p0)/lane_length; - const auto proj = (p - p0).dot(lane_u); - if (proj < 0.0 || lane_length < proj) - { - continue; - } - 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); } } } - if (starts.empty()) + if (on_lane.has_value()) { - starts = nav_params->compute_plan_starts(graph, map, location, now); - } - - if (!starts.empty()) - { - context->set_location(starts); - } - else - { - context->set_lost(Location { now, map, location }); - } - - if (!waypoints.empty()) - { - 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()) + 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)) { - 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); - } + starts.push_back(rmf_traffic::agv::Plan::Start( + now, wp0, yaw, p, reverse_lane->index())); } - - 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); } } - }; - using DataPtr = std::shared_ptr; - std::weak_ptr w_context; - std::shared_ptr data; - std::function begin; - std::function finisher; - ActivityIdentifierPtr identifier; + if (starts.empty()) + { + starts = nav_params->compute_plan_starts(graph, map, location, now); + } - void finish() - { - if (auto context = w_context.lock()) + if (!starts.empty()) { - context->worker().schedule( - [ - context = context, - data = this->data, - identifier = this->identifier, - finisher = this->finisher - ](const auto&) + context->set_location(starts); + } + else + { + context->set_lost(Location { now, map, location }); + } + + if (!waypoints.empty()) + { + 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()) + { + 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->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()) + { + 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() @@ -1437,28 +1455,7 @@ void EasyFullControl::EasyRobotUpdateHandle::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(); - 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 }); - } + updater->nav_params->search_for_location(state.map(), position, *context); }); } @@ -2583,7 +2580,7 @@ auto EasyFullControl::add_robot( localization = [ inner = callbacks.localize(), nav_params = robot_nav_params - ](Destination estimate) + ](Destination estimate, CommandExecution execution) { auto robot_position = nav_params->to_robot_coordinates( estimate.map(), @@ -2595,7 +2592,7 @@ auto EasyFullControl::add_robot( *robot_position, estimate.graph_index(), estimate.speed_limit()); - inner(robot_estimate); + inner(robot_estimate, execution); } }; } 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 faadb5887..8081cc79a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -28,6 +28,34 @@ 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()) + { + context.set_location(std::move(starts)); + } + else + { + context.set_lost(Location { now, map, position }); + } +} + //============================================================================== std::shared_ptr RobotContext::command() { @@ -623,10 +651,17 @@ const Reporting& RobotContext::reporting() const } //============================================================================== -void RobotContext::localize(EasyFullControl::Destination estimate) const +bool RobotContext::localize( + EasyFullControl::Destination estimate, + EasyFullControl::CommandExecution execution) const { if (_localize) - _localize(std::move(estimate)); + { + _localize(std::move(estimate), std::move(execution)); + return true; + } + + return false; } //============================================================================== 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 cb5fcb188..215eb55ec 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -50,6 +50,7 @@ class TaskManager; namespace agv { +class RobotContext; using TransformDictionary = std::unordered_map; using SharedPlanner = std::shared_ptr< std::shared_ptr>; @@ -101,6 +102,11 @@ struct NavParams 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, @@ -381,7 +387,9 @@ class RobotContext const Reporting& reporting() const; /// Tell the robot to localize near here - void localize(EasyFullControl::Destination estimate) const; + bool localize( + EasyFullControl::Destination estimate, + EasyFullControl::CommandExecution execution) const; /// Set the callback for localizing the robot void set_localization(EasyFullControl::LocalizationRequest localization); 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 ebcb68a3c..9f20be5eb 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; @@ -87,5 +89,41 @@ class EasyFullControl::Destination::Implementation } }; +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_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index e8c8b24f4..25c3d54e9 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: 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 c022674fe..b53907165 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -93,11 +93,13 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor agv::RobotContextPtr context, LegacyPhases& phases, const rmf_traffic::agv::Plan::Waypoint& waypoint_, + rmf_traffic::PlanId plan_id, bool& continuous) : waypoint(waypoint_), _context(std::move(context)), _phases(phases), _event_start_time(waypoint_.time()), + _plan_id(plan_id), _continuous(continuous) { // Do nothing @@ -154,7 +156,8 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.lift_name(), open.floor_name(), _event_start_time, - phases::RequestLift::Located::Outside), + phases::RequestLift::Located::Outside, + _plan_id), _event_start_time, waypoint.dependencies()); _continuous = true; @@ -188,6 +191,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.floor_name(), _event_start_time + open.duration() + _lifting_duration, phases::RequestLift::Located::Inside, + _plan_id, localize), _event_start_time, waypoint.dependencies()); _moving_lift = false; @@ -223,6 +227,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor agv::RobotContextPtr _context; LegacyPhases& _phases; rmf_traffic::Time _event_start_time; + rmf_traffic::PlanId _plan_id; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); @@ -483,7 +488,8 @@ std::optional ExecutePlan::make( move_through.clear(); bool continuous = true; - EventPhaseFactory factory(context, legacy_phases, *it, continuous); + EventPhaseFactory factory( + context, legacy_phases, *it, plan_id, continuous); it->event()->execute(factory); while (factory.moving_lift()) { 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 61a4fd2c0..260e1c28d 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 { @@ -29,6 +31,7 @@ std::shared_ptr RequestLift::ActivePhase::make( std::string destination, rmf_traffic::Time expected_finish, const Located located, + rmf_traffic::PlanId plan_id, std::optional localize) { auto inst = std::shared_ptr( @@ -38,6 +41,7 @@ std::shared_ptr RequestLift::ActivePhase::make( std::move(destination), std::move(expected_finish), located, + plan_id, std::move(localize) )); inst->_init_obs(); @@ -83,12 +87,14 @@ RequestLift::ActivePhase::ActivePhase( std::string destination, rmf_traffic::Time expected_finish, Located located, + rmf_traffic::PlanId plan_id, std::optional localize) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), _expected_finish(std::move(expected_finish)), _located(located), + _plan_id(plan_id), _localize_after(std::move(localize)) { std::ostringstream oss; @@ -124,17 +130,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->_expected_finish; if (delay > std::chrono::seconds(0)) { me->_context->worker().schedule( - [context = me->_context, delay](const auto&) + [ + context = me->_context, + plan_id = me->_plan_id, + delay + ](const auto&) { - context->itinerary().delay(delay); + context->itinerary().cumulative_delay(plan_id, delay); }); } }); @@ -173,7 +179,42 @@ void RequestLift::ActivePhase::_init_obs() if (me->_localize_after.has_value()) { - me->_context->localize(*me->_localize_after); + auto finish = [s, worker = me->_context->worker()]() + { + worker.schedule([s](const auto&) + { + s.on_completed(); + }); + }; + auto cmd = agv::EasyFullControl + ::CommandExecution::Implementation::make_hold( + me->_context, + me->_expected_finish, + me->_plan_id, + std::move(finish)); + + if (me->_context->localize(*me->_localize_after, std::move(cmd))) + { + me->_rewait_timer = me->_context->node()->try_create_wall_timer( + std::chrono::seconds(30), + [w = me->weak_from_this(), s] + { + const auto me = w.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()); + + s.on_completed(); + }); + return; + } } if (!me->_cancelled.get_value() || me->_located == Located::Inside) @@ -330,12 +371,14 @@ RequestLift::PendingPhase::PendingPhase( std::string destination, rmf_traffic::Time expected_finish, Located located, + rmf_traffic::PlanId plan_id, std::optional localize) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), _expected_finish(std::move(expected_finish)), _located(located), + _plan_id(plan_id), _localize_after(std::move(localize)) { std::ostringstream oss; @@ -353,6 +396,7 @@ std::shared_ptr RequestLift::PendingPhase::begin() _destination, _expected_finish, _located, + _plan_id, _localize_after); } 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 6154080aa..0b6f34822 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -45,6 +45,7 @@ struct RequestLift std::string destination, rmf_traffic::Time expected_finish, Located located, + rmf_traffic::PlanId plan_id, std::optional localize); const rxcpp::observable& observe() const override; @@ -70,6 +71,7 @@ struct RequestLift rclcpp::TimerBase::SharedPtr _timer; std::shared_ptr _lift_end_phase; Located _located; + rmf_traffic::PlanId _plan_id; std::optional _localize_after; rmf_rxcpp::subscription_guard _reset_session_subscription; @@ -89,6 +91,7 @@ struct RequestLift std::string destination, rmf_traffic::Time expected_finish, Located located, + rmf_traffic::PlanId plan_id, std::optional localize); void _init_obs(); @@ -109,6 +112,7 @@ struct RequestLift std::string destination, rmf_traffic::Time expected_finish, Located located, + rmf_traffic::PlanId plan_id, std::optional localize = std::nullopt); std::shared_ptr begin() override; @@ -128,6 +132,7 @@ struct RequestLift std::string _destination; rmf_traffic::Time _expected_finish; Located _located; + rmf_traffic::PlanId _plan_id; std::optional _localize_after; std::string _description; }; diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index 6e2a76c1f..1d58a907e 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -71,7 +71,8 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") lift_name, destination, context->now() + std::chrono::seconds(5), - RequestLift::Located::Outside + RequestLift::Located::Outside, + 0 ); auto active_phase = pending_phase->begin(); From 9221f65568e468ae1199465d8e51b9a318f7131c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 25 Sep 2023 20:01:59 +0800 Subject: [PATCH 09/89] Fix segfault Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 01ec4e022..f4adcf125 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -518,7 +518,7 @@ void EasyFullControl::CommandExecution::Implementation::finish() // Prevent this activity from doing any further updates ActivityIdentifier::Implementation::get( *identifier).update_fn = nullptr; - if (data->schedule_override.has_value()) + if (data && data->schedule_override.has_value()) { data->release_stubbornness(); RCLCPP_INFO( From 2b5b9333868b75b30541da3af46442c134ff2060 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 27 Sep 2023 03:16:46 +0000 Subject: [PATCH 10/89] Align the waypoints along each lift shaft Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 36 ++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) 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..208b86e79 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 @@ -309,8 +309,9 @@ rmf_traffic::agv::Graph parse_graph( 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 +328,39 @@ 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); + } } } From 11ba2a4c5497622572f26ba6b722f30df8468bb2 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 25 Sep 2023 11:28:24 +0800 Subject: [PATCH 11/89] Add API to retrieve robot task ID from the RobotUpdateHandle Signed-off-by: Xiyu Oh --- .../rmf_fleet_adapter/agv/RobotUpdateHandle.hpp | 4 ++++ .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 15 +++++++++++++++ .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 4 ++++ .../rmf_fleet_adapter/agv/RobotUpdateHandle.cpp | 9 +++++++++ rmf_fleet_adapter_python/src/adapter.cpp | 5 +++++ 5 files changed, 37 insertions(+) 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..2b3138c3e 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 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 8081cc79a..05b238c20 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -474,6 +474,21 @@ RobotContext& RobotContext::current_task_id(std::optional id) return *this; } +//============================================================================== +const std::string RobotContext::copy_current_task_id() const +{ + std::mutex _mutex; + std::unique_lock lock(); + + { + std::unique_lock lock(_mutex); + if (_current_task_id.has_value()) + return _current_task_id.value(); + + return {}; + } +} + //============================================================================== double RobotContext::current_battery_soc() const { 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 215eb55ec..11478178f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -320,6 +320,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 + const std::string copy_current_task_id() const; + /// Get the current battery state of charge double current_battery_soc() const; 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..ba186d0f0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -331,6 +331,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) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index d6b1b5afb..a01b52c90 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) { From 2bc79e3c391800e98bbe1f062a957163947482bc Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 28 Sep 2023 17:12:19 +0800 Subject: [PATCH 12/89] Bug fixes Signed-off-by: Xiyu Oh --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp | 2 +- rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp | 2 +- 3 files changed, 3 insertions(+), 3 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 c95e17123..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); } 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/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index 260e1c28d..c32977008 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -209,7 +209,7 @@ void RequestLift::ActivePhase::_init_obs() "ensure that your localization function triggers " "execution.finished() when the robot's localization " "process is finished.", - me->_context->requester_id()); + me->_context->requester_id().c_str()); s.on_completed(); }); From 5efa1d4606c4102332030dce526298ca1dcc488f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 2 Oct 2023 08:42:36 +0000 Subject: [PATCH 13/89] Always move robot location estimates to the bottom of a vertex stack Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 1 + .../rmf_fleet_adapter/agv/RobotContext.cpp | 126 ++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 13 +- 3 files changed, 139 insertions(+), 1 deletion(-) 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 f4adcf125..82a56df1b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -2686,6 +2686,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); 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 05b238c20..f69687fd4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -56,6 +56,132 @@ void NavParams::search_for_location( } } +//============================================================================== +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; + } + } +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + for (rmf_traffic::agv::Plan::Start& location : locations) + { + 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; + + std::size_t waypoint = waypoint_opt.value(); + 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. + location.lane(std::nullopt); + location.waypoint(waypoint); + } +} + //============================================================================== std::shared_ptr RobotContext::command() { 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 11478178f..c3e1b32da 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -43,6 +43,8 @@ #include "Node.hpp" #include "../Reporting.hpp" +#include + namespace rmf_fleet_adapter { // Forward declaration @@ -55,6 +57,7 @@ using TransformDictionary = std::unordered_map; using SharedPlanner = std::shared_ptr< std::shared_ptr>; using Destination = EasyFullControl::Destination; +using VertexStack = std::shared_ptr>; //============================================================================== struct NavParams @@ -125,11 +128,19 @@ struct NavParams min_lane_length); if (!starts.empty()) - return starts; + return descend_stacks(graph, starts); } return {}; } + + std::unordered_map stacked_vertices; + + void find_stacked_vertices(const rmf_traffic::agv::Graph& graph); + + rmf_traffic::agv::Plan::StartSet descend_stacks( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; }; //============================================================================== From bd8dc72862fd74dfa5aefebf773616f39f90515c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 3 Oct 2023 08:20:18 +0000 Subject: [PATCH 14/89] Debugging behavior inside of lifts Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 4 + .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 142 ++++++++++++++++-- .../rmf_fleet_adapter/agv/RobotContext.cpp | 63 ++++++-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 2 + .../agv/internal_EasyFullControl.hpp | 6 + .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 17 ++- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 33 ++++ .../rmf_fleet_adapter/phases/RequestLift.cpp | 3 + rmf_fleet_adapter_python/src/adapter.cpp | 5 +- 9 files changed, 252 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 4f227c8f3..2a460808e 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -446,6 +446,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 name of + /// the lift. + std::optional inside_lift() const; + class Implementation; private: Destination(); 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 82a56df1b..a3c328053 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -718,6 +718,12 @@ std::optional EasyFullControl::Destination::dock() const return _pimpl->dock; } +//============================================================================== +std::optional EasyFullControl::Destination::inside_lift() const +{ + return _pimpl->lift; +} + //============================================================================== EasyFullControl::Destination::Destination() { @@ -880,9 +886,27 @@ void EasyCommandHandle::stop() this->handle_stop(activity_identifier); } +class Printer : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + Printer() + { + // Do nothing + } + + void execute(const DoorOpen&) override { std::cout << __LINE__; } + void execute(const DoorClose&) override { std::cout << __LINE__; } + void execute(const LiftSessionBegin&) override { std::cout << __LINE__; } + void execute(const LiftDoorOpen&) override { std::cout << __LINE__; } + void execute(const LiftSessionEnd&) override { std::cout << __LINE__; } + void execute(const LiftMove&) override { std::cout << __LINE__; } + void execute(const Wait&) override { std::cout << __LINE__; } + void execute(const Dock& dock) override { std::cout << __LINE__; } +}; + //============================================================================== void EasyCommandHandle::follow_new_path( - const std::vector& waypoints, + const std::vector& cmd_waypoints, ArrivalEstimator next_arrival_estimator_, RequestCompleted path_finished_callback_) { @@ -898,7 +922,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) { @@ -920,6 +944,44 @@ void EasyCommandHandle::follow_new_path( return; } const auto& graph = planner->get_configuration().graph(); + std::vector waypoints = cmd_waypoints; + std::size_t i_flatten = 1; + while (i_flatten < waypoints.size()) + { + const auto& wp0 = waypoints[i_flatten-1]; + const auto& wp1 = waypoints[i_flatten]; + if (!wp0.graph_index().has_value() || !wp1.graph_index().has_value()) + { + continue; + } + + const auto i0 = *wp0.graph_index(); + const auto i1 = *wp1.graph_index(); + if (nav_params->in_same_stack(i0, i1)) + { + const auto yaw_diff = rmf_utils::wrap_to_pi( + wp0.position()[2] - wp1.position()[2]); + if (std::abs(yaw_diff) < 5*M_PI/180.0) + { + // These waypoints are redundant for moving so let's filter one out + waypoints.erase(waypoints.begin() + i_flatten); + continue; + } + } + + ++i_flatten; + } + + 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) { @@ -956,6 +1018,55 @@ void EasyCommandHandle::follow_new_path( std::vector queue; const auto& current_location = context->location(); + std::cout << "Locations: " << current_location.size() << std::endl; + for (const auto& l : current_location) + { + std::cout << " -- "; + if (l.lane().has_value()) + { + std::cout << "lane[" << *l.lane() << "] "; + Printer printer; + const auto& lane = graph.get_lane(*l.lane()); + if (lane.entry().event()) + { + std::cout << " [entry "; + lane.entry().event()->execute(printer); + std::cout << "] "; + } + const auto& i_wp0 = lane.entry().waypoint_index(); + const auto& wp0 = graph.get_waypoint(i_wp0); + const auto& i_wp1 = lane.exit().waypoint_index(); + const auto& wp1 = graph.get_waypoint(i_wp1); + std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() + << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + if (lane.exit().event()) + { + std::cout << "[exit "; + lane.exit().event()->execute(printer); + std::cout << "]"; + } + std::cout << " | "; + } + std::cout << l.waypoint(); + if (l.location().has_value()) + { + std::cout << " | " << l.location()->transpose(); + } + std::cout << std::endl; + } + std::cout << "Num waypoints: " << waypoints.size() << std::endl; + for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) + { + std::cout << " --"; + if (wp.graph_index().has_value()) + { + std::cout << " index " << *wp.graph_index(); + const auto& gwp = graph.get_waypoint(*wp.graph_index()); + std::cout << " [" << gwp.get_map_name() << "]"; + } + std::cout << " " << wp.position().transpose() << std::endl; + } + bool found_connection = false; std::size_t i0 = 0; for (std::size_t i = 0; i < waypoints.size(); ++i) @@ -965,7 +1076,7 @@ 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,6 +1089,7 @@ void EasyCommandHandle::follow_new_path( { const double dist = (*l.location() - wp.position().block<2, 1>(0, 0)).norm(); + std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1007,6 +1119,7 @@ void EasyCommandHandle::follow_new_path( p_wp = graph.get_waypoint(l.waypoint()).get_location(); } const double dist = (*l.location() - p_wp).norm(); + std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1133,12 +1246,21 @@ void EasyCommandHandle::follow_new_path( } } + std::optional in_lift; + if (wp1.graph_index().has_value()) + { + in_lift = rmf_utils::pointer_to_opt( + graph.get_waypoint(*wp1.graph_index()).in_lift()); + } + 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); + speed_limit, + in_lift); + queue.push_back( EasyFullControl::CommandExecution::Implementation::make( @@ -1324,6 +1446,7 @@ void EasyCommandHandle::dock( command_position, i1, lane.properties().speed_limit(), + rmf_utils::pointer_to_opt(wp1.in_lift()), dock_name_); auto cmd = EasyFullControl::CommandExecution::Implementation::make( @@ -2587,12 +2710,11 @@ auto EasyFullControl::add_robot( estimate.position()); if (robot_position.has_value()) { - auto robot_estimate = Destination::Implementation::make( - estimate.map(), - *robot_position, - estimate.graph_index(), - estimate.speed_limit()); - inner(robot_estimate, execution); + auto transformed_estimate = estimate; + Destination::Implementation::get(transformed_estimate) + .position = *robot_position; + + inner(transformed_estimate, execution); } }; } 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 f69687fd4..99a0e4309 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -163,6 +163,7 @@ rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( { if (graph.lane_from(v, waypoint)) { + std::cout << "descending " << v << " -> " << waypoint << std::endl; waypoint = v; can_descend = true; break; @@ -177,9 +178,33 @@ rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( // Transfer the location estimate over to the waypoint that's at the bottom // of the vertex stack. + std::cout << "Descended vertex stack to " << waypoint << std::endl; location.lane(std::nullopt); location.waypoint(waypoint); } + + 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); } //============================================================================== @@ -197,25 +222,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() + "]"); } //============================================================================== 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 c3e1b32da..b93ce5854 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -141,6 +141,8 @@ struct NavParams rmf_traffic::agv::Plan::StartSet descend_stacks( 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; }; //============================================================================== 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 9f20be5eb..ba240cecc 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 @@ -77,6 +77,7 @@ class EasyFullControl::Destination::Implementation Eigen::Vector3d position; std::optional graph_index; std::optional speed_limit; + std::optional lift; std::optional dock = std::nullopt; template @@ -87,6 +88,11 @@ class EasyFullControl::Destination::Implementation Implementation{std::forward(args)...}); return output; } + + static Implementation& get(Destination& self) + { + return *self._pimpl; + } }; class EasyFullControl::CommandExecution::Implementation 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 208b86e79..928ca5c6e 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,6 +19,8 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -59,6 +61,7 @@ rmf_traffic::agv::Graph parse_graph( 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) @@ -133,6 +136,7 @@ rmf_traffic::agv::Graph parse_graph( { wps_of_lift[lift_name].push_back(wp.index()); lift_of_wp[wp.index()] = lift_name; + wp.set_in_lift(lift_name); } } } @@ -140,7 +144,6 @@ rmf_traffic::agv::Graph parse_graph( const YAML::Node& lanes = level.second["lanes"]; for (const auto& lane : lanes) { - ConstraintPtr constraint = nullptr; const YAML::Node& options = lane[2]; @@ -284,6 +287,12 @@ rmf_traffic::agv::Graph parse_graph( graph.add_lane( {begin, entry_event}, {dock_wp.index(), rmf_utils::clone_ptr()}); + stacked_vertex.insert({begin, dock_wp.index()}); + + if (const auto* lift_name = graph.get_waypoint(begin).in_lift()) + { + dock_wp.set_in_lift(*lift_name); + } // First lane from start -> dock, second lane from dock -> end begin = dock_wp.index(); @@ -360,6 +369,12 @@ rmf_traffic::agv::Graph parse_graph( 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/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index b53907165..1020d4d8f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -435,6 +435,24 @@ std::optional search_for_lift_group( } // anonymous namespace +class Printer : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + Printer() + { + // Do nothing + } + + void execute(const DoorOpen&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const DoorClose&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const LiftSessionBegin&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const LiftDoorOpen&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const LiftSessionEnd&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const LiftMove&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const Wait&) override { std::cout << "event " << __LINE__ << std::endl;; } + void execute(const Dock& dock) override { std::cout << "event " << __LINE__ << std::endl;; } +}; + //============================================================================== std::optional ExecutePlan::make( agv::RobotContextPtr context, @@ -447,6 +465,21 @@ std::optional ExecutePlan::make( std::function finished, std::optional tail_period) { + std::cout << " --- plan --- " << std::endl; + const auto t0 = plan.get_waypoints().front().time(); + for (const rmf_traffic::agv::Plan::Waypoint& wp : plan.get_waypoints()) + { + std::cout << " -- t=" << rmf_traffic::time::to_seconds(wp.time() - t0) << " "; + if (wp.graph_index().has_value()) + std::cout << "index " << *wp.graph_index() << " "; + std::cout << "[" << wp.position().transpose() << "]" << std::endl; + if (wp.event()) + { + Printer printer; + wp.event()->execute(printer); + } + } + std::optional finish_time_estimate; for (const auto& r : plan.get_itinerary()) { 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 c32977008..56081fd7b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -193,6 +193,9 @@ void RequestLift::ActivePhase::_init_obs() me->_plan_id, std::move(finish)); + agv::Destination::Implementation::get(*me->_localize_after) + .position = me->_context->position(); + if (me->_context->localize(*me->_localize_after, std::move(cmd))) { me->_rewait_timer = me->_context->node()->try_create_wall_timer( diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index a01b52c90..620a3ec8b 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -830,7 +830,7 @@ PYBIND11_MODULE(rmf_adapter, m) { .def( "override_schedule", []( - ActionExecution& self, + agv::EasyFullControl::CommandExecution& self, std::string map, std::vector path, double hold) @@ -852,7 +852,8 @@ PYBIND11_MODULE(rmf_adapter, m) { .def_property_readonly("yaw", &agv::EasyFullControl::Destination::yaw) .def_property_readonly("graph_index", &agv::EasyFullControl::Destination::graph_index) .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 From 1a16a6acb9d763ce764a167432ebe82f9dc19e18 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 3 Oct 2023 08:42:17 +0000 Subject: [PATCH 15/89] Fix infinite loop Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 1 + 1 file changed, 1 insertion(+) 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 a3c328053..de5616dcf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -952,6 +952,7 @@ void EasyCommandHandle::follow_new_path( const auto& wp1 = waypoints[i_flatten]; if (!wp0.graph_index().has_value() || !wp1.graph_index().has_value()) { + ++i_flatten; continue; } From ee674435718ceef6392b3e2e731e339f04c49a0f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 4 Oct 2023 03:32:25 +0000 Subject: [PATCH 16/89] Only remove lane if descended Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 76 ++++++++++++++++++- 1 file changed, 73 insertions(+), 3 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 99a0e4309..c31f32dd5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -137,7 +137,8 @@ rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( if (!waypoint_opt.has_value()) continue; - std::size_t waypoint = waypoint_opt.value(); + 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()) @@ -179,8 +180,11 @@ rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( // Transfer the location estimate over to the waypoint that's at the bottom // of the vertex stack. std::cout << "Descended vertex stack to " << waypoint << std::endl; - location.lane(std::nullopt); - location.waypoint(waypoint); + if (waypoint != original_waypoint) + { + location.lane(std::nullopt); + location.waypoint(waypoint); + } } return locations; @@ -279,6 +283,24 @@ const rmf_traffic::agv::Plan::StartSet& RobotContext::location() const return _location; } +class Printer : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + Printer() + { + // Do nothing + } + + void execute(const DoorOpen&) override { std::cout << __LINE__; } + void execute(const DoorClose&) override { std::cout << __LINE__; } + void execute(const LiftSessionBegin&) override { std::cout << __LINE__; } + void execute(const LiftDoorOpen&) override { std::cout << __LINE__; } + void execute(const LiftSessionEnd&) override { std::cout << __LINE__; } + void execute(const LiftMove&) override { std::cout << __LINE__; } + void execute(const Wait&) override { std::cout << __LINE__; } + void execute(const Dock& dock) override { std::cout << __LINE__; } +}; + //============================================================================== void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) { @@ -289,6 +311,54 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) _location = std::move(location_); filter_closed_lanes(); + + const auto& graph = navigation_graph(); + for (const auto& l : _location) + { + std::cout << " -- "; + if (l.lane().has_value()) + { + std::cout << "lane[" << *l.lane() << "] "; + Printer printer; + const auto& lane = graph.get_lane(*l.lane()); + if (lane.entry().event()) + { + std::cout << " [entry "; + lane.entry().event()->execute(printer); + std::cout << "] "; + } + const auto& i_wp0 = lane.entry().waypoint_index(); + const auto& wp0 = graph.get_waypoint(i_wp0); + const auto& i_wp1 = lane.exit().waypoint_index(); + const auto& wp1 = graph.get_waypoint(i_wp1); + std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; + if (const auto* lift = wp0.in_lift()) + std::cout << "{" << *lift << "} "; + std::cout << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + if (const auto* lift = wp1.in_lift()) + std::cout << "{" << *lift << "} "; + + if (lane.exit().event()) + { + std::cout << "[exit "; + lane.exit().event()->execute(printer); + std::cout << "]"; + } + std::cout << " | "; + } + std::cout << l.waypoint(); + const auto& wp = graph.get_waypoint(l.waypoint()); + if (const auto* lift = wp.in_lift()) + { + std::cout << " {" << *lift << "}"; + } + if (l.location().has_value()) + { + std::cout << " | " << l.location()->transpose(); + } + std::cout << std::endl; + } + if (_location.empty()) { set_lost(std::nullopt); From 25a08b4e9936db1c01c5f879bcc4c3d181f1b274 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 4 Oct 2023 09:33:43 +0000 Subject: [PATCH 17/89] Include lift properties in nav graph Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 6 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 11 ++- .../rmf_fleet_adapter/agv/RobotContext.cpp | 76 +++++++++++++++++-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 14 +++- .../agv/internal_EasyFullControl.hpp | 2 +- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 54 ++++++++++++- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 2 +- rmf_fleet_adapter_python/src/graph/graph.cpp | 11 +++ 8 files changed, 153 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 2a460808e..43e12a0c2 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -446,9 +446,9 @@ 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 name of - /// the lift. - std::optional inside_lift() 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: 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 de5616dcf..70453c10f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -719,7 +719,8 @@ std::optional EasyFullControl::Destination::dock() const } //============================================================================== -std::optional EasyFullControl::Destination::inside_lift() const +rmf_traffic::agv::Graph::LiftPropertiesPtr +EasyFullControl::Destination::inside_lift() const { return _pimpl->lift; } @@ -1247,11 +1248,10 @@ void EasyCommandHandle::follow_new_path( } } - std::optional in_lift; + rmf_traffic::agv::Graph::LiftPropertiesPtr in_lift; if (wp1.graph_index().has_value()) { - in_lift = rmf_utils::pointer_to_opt( - graph.get_waypoint(*wp1.graph_index()).in_lift()); + in_lift = graph.get_waypoint(*wp1.graph_index()).in_lift(); } const auto command_position = to_robot_coordinates(map, target_position); @@ -1262,7 +1262,6 @@ void EasyCommandHandle::follow_new_path( speed_limit, in_lift); - queue.push_back( EasyFullControl::CommandExecution::Implementation::make( context, @@ -1447,7 +1446,7 @@ void EasyCommandHandle::dock( command_position, i1, lane.properties().speed_limit(), - rmf_utils::pointer_to_opt(wp1.in_lift()), + wp1.in_lift(), dock_name_); auto cmd = EasyFullControl::CommandExecution::Implementation::make( 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 c31f32dd5..e959e0281 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -116,7 +116,15 @@ void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) } //============================================================================== -rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( +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 { @@ -190,6 +198,60 @@ rmf_traffic::agv::Plan::StartSet NavParams::descend_stacks( 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.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, @@ -332,11 +394,11 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) const auto& i_wp1 = lane.exit().waypoint_index(); const auto& wp1 = graph.get_waypoint(i_wp1); std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; - if (const auto* lift = wp0.in_lift()) - std::cout << "{" << *lift << "} "; + if (const auto lift = wp0.in_lift()) + std::cout << "{" << lift->name() << "} "; std::cout << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - if (const auto* lift = wp1.in_lift()) - std::cout << "{" << *lift << "} "; + if (const auto lift = wp1.in_lift()) + std::cout << "{" << lift->name() << "} "; if (lane.exit().event()) { @@ -348,9 +410,9 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) } std::cout << l.waypoint(); const auto& wp = graph.get_waypoint(l.waypoint()); - if (const auto* lift = wp.in_lift()) + if (const auto lift = wp.in_lift()) { - std::cout << " {" << *lift << "}"; + std::cout << " {" << lift->name() << "}"; } if (l.location().has_value()) { 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 b93ce5854..3cef40712 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -128,7 +128,7 @@ struct NavParams min_lane_length); if (!starts.empty()) - return descend_stacks(graph, starts); + return process_locations(graph, starts); } return {}; @@ -138,7 +138,17 @@ struct NavParams void find_stacked_vertices(const rmf_traffic::agv::Graph& graph); - rmf_traffic::agv::Plan::StartSet descend_stacks( + 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; 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 ba240cecc..038b35093 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 @@ -77,7 +77,7 @@ class EasyFullControl::Destination::Implementation Eigen::Vector3d position; std::optional graph_index; std::optional speed_limit; - std::optional lift; + rmf_traffic::agv::Graph::LiftPropertiesPtr lift; std::optional dock = std::nullopt; template 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 928ca5c6e..7c9e07c5c 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 @@ -24,6 +24,8 @@ namespace rmf_fleet_adapter { namespace agv { +using LiftPropertiesPtr = rmf_traffic::agv::Graph::LiftPropertiesPtr; + //============================================================================== rmf_traffic::agv::Graph parse_graph( const std::string& graph_file, @@ -35,6 +37,41 @@ rmf_traffic::agv::Graph parse_graph( throw std::runtime_error("Failed to load graph file [" + graph_file + "]"); } + std::unordered_map lifts; + 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()); + + lifts[name] = std::make_shared( + name, location, orientation, dimensions); + } + } + const YAML::Node levels = graph_config["levels"]; if (!levels) { @@ -136,7 +173,18 @@ rmf_traffic::agv::Graph parse_graph( { wps_of_lift[lift_name].push_back(wp.index()); lift_of_wp[wp.index()] = lift_name; - wp.set_in_lift(lift_name); + if (has_lifts) + { + const auto l_it = lifts.find(lift_name); + if (l_it == lifts.end()) + { + 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(l_it->second); + } } } } @@ -289,9 +337,9 @@ rmf_traffic::agv::Graph parse_graph( {dock_wp.index(), rmf_utils::clone_ptr()}); stacked_vertex.insert({begin, dock_wp.index()}); - if (const auto* lift_name = graph.get_waypoint(begin).in_lift()) + if (const auto lift = graph.get_waypoint(begin).in_lift()) { - dock_wp.set_in_lift(*lift_name); + dock_wp.set_in_lift(lift); } // First lane from start -> dock, second lane from dock -> end 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 1020d4d8f..1dcf5ce9f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -181,7 +181,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor waypoint.position(), waypoint.graph_index(), std::nullopt, - std::nullopt); + nullptr); // TODO(MXG): The time calculation here should be considered more carefully. _phases.emplace_back( diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index a540c2408..c01a7cabf 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") From bb5ea4f0175c8d31b770755b17a7c5c965332f90 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 5 Oct 2023 07:55:28 +0000 Subject: [PATCH 18/89] Remove debug output Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 100 +++++++++--------- .../rmf_fleet_adapter/agv/RobotContext.cpp | 96 ++++++++--------- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 28 ++--- 3 files changed, 112 insertions(+), 112 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 70453c10f..daaec84ed 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1020,54 +1020,54 @@ void EasyCommandHandle::follow_new_path( std::vector queue; const auto& current_location = context->location(); - std::cout << "Locations: " << current_location.size() << std::endl; - for (const auto& l : current_location) - { - std::cout << " -- "; - if (l.lane().has_value()) - { - std::cout << "lane[" << *l.lane() << "] "; - Printer printer; - const auto& lane = graph.get_lane(*l.lane()); - if (lane.entry().event()) - { - std::cout << " [entry "; - lane.entry().event()->execute(printer); - std::cout << "] "; - } - const auto& i_wp0 = lane.entry().waypoint_index(); - const auto& wp0 = graph.get_waypoint(i_wp0); - const auto& i_wp1 = lane.exit().waypoint_index(); - const auto& wp1 = graph.get_waypoint(i_wp1); - std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() - << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - if (lane.exit().event()) - { - std::cout << "[exit "; - lane.exit().event()->execute(printer); - std::cout << "]"; - } - std::cout << " | "; - } - std::cout << l.waypoint(); - if (l.location().has_value()) - { - std::cout << " | " << l.location()->transpose(); - } - std::cout << std::endl; - } - std::cout << "Num waypoints: " << waypoints.size() << std::endl; - for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) - { - std::cout << " --"; - if (wp.graph_index().has_value()) - { - std::cout << " index " << *wp.graph_index(); - const auto& gwp = graph.get_waypoint(*wp.graph_index()); - std::cout << " [" << gwp.get_map_name() << "]"; - } - std::cout << " " << wp.position().transpose() << std::endl; - } + // std::cout << "Locations: " << current_location.size() << std::endl; + // for (const auto& l : current_location) + // { + // std::cout << " -- "; + // if (l.lane().has_value()) + // { + // std::cout << "lane[" << *l.lane() << "] "; + // Printer printer; + // const auto& lane = graph.get_lane(*l.lane()); + // if (lane.entry().event()) + // { + // std::cout << " [entry "; + // lane.entry().event()->execute(printer); + // std::cout << "] "; + // } + // const auto& i_wp0 = lane.entry().waypoint_index(); + // const auto& wp0 = graph.get_waypoint(i_wp0); + // const auto& i_wp1 = lane.exit().waypoint_index(); + // const auto& wp1 = graph.get_waypoint(i_wp1); + // std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() + // << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + // if (lane.exit().event()) + // { + // std::cout << "[exit "; + // lane.exit().event()->execute(printer); + // std::cout << "]"; + // } + // std::cout << " | "; + // } + // std::cout << l.waypoint(); + // if (l.location().has_value()) + // { + // std::cout << " | " << l.location()->transpose(); + // } + // std::cout << std::endl; + // } + // std::cout << "Num waypoints: " << waypoints.size() << std::endl; + // for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) + // { + // std::cout << " --"; + // if (wp.graph_index().has_value()) + // { + // std::cout << " index " << *wp.graph_index(); + // const auto& gwp = graph.get_waypoint(*wp.graph_index()); + // std::cout << " [" << gwp.get_map_name() << "]"; + // } + // std::cout << " " << wp.position().transpose() << std::endl; + // } bool found_connection = false; std::size_t i0 = 0; @@ -1091,7 +1091,7 @@ void EasyCommandHandle::follow_new_path( { const double dist = (*l.location() - wp.position().block<2, 1>(0, 0)).norm(); - std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; + // std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1121,7 +1121,7 @@ void EasyCommandHandle::follow_new_path( p_wp = graph.get_waypoint(l.waypoint()).get_location(); } const double dist = (*l.location() - p_wp).norm(); - std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; + // std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; 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 e959e0281..6f23a82db 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -172,7 +172,7 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( { if (graph.lane_from(v, waypoint)) { - std::cout << "descending " << v << " -> " << waypoint << std::endl; + // std::cout << "descending " << v << " -> " << waypoint << std::endl; waypoint = v; can_descend = true; break; @@ -187,7 +187,7 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( // Transfer the location estimate over to the waypoint that's at the bottom // of the vertex stack. - std::cout << "Descended vertex stack to " << waypoint << std::endl; + // std::cout << "Descended vertex stack to " << waypoint << std::endl; if (waypoint != original_waypoint) { location.lane(std::nullopt); @@ -374,52 +374,52 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) _location = std::move(location_); filter_closed_lanes(); - const auto& graph = navigation_graph(); - for (const auto& l : _location) - { - std::cout << " -- "; - if (l.lane().has_value()) - { - std::cout << "lane[" << *l.lane() << "] "; - Printer printer; - const auto& lane = graph.get_lane(*l.lane()); - if (lane.entry().event()) - { - std::cout << " [entry "; - lane.entry().event()->execute(printer); - std::cout << "] "; - } - const auto& i_wp0 = lane.entry().waypoint_index(); - const auto& wp0 = graph.get_waypoint(i_wp0); - const auto& i_wp1 = lane.exit().waypoint_index(); - const auto& wp1 = graph.get_waypoint(i_wp1); - std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; - if (const auto lift = wp0.in_lift()) - std::cout << "{" << lift->name() << "} "; - std::cout << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - if (const auto lift = wp1.in_lift()) - std::cout << "{" << lift->name() << "} "; - - if (lane.exit().event()) - { - std::cout << "[exit "; - lane.exit().event()->execute(printer); - std::cout << "]"; - } - std::cout << " | "; - } - std::cout << l.waypoint(); - const auto& wp = graph.get_waypoint(l.waypoint()); - if (const auto lift = wp.in_lift()) - { - std::cout << " {" << lift->name() << "}"; - } - if (l.location().has_value()) - { - std::cout << " | " << l.location()->transpose(); - } - std::cout << std::endl; - } + // const auto& graph = navigation_graph(); + // for (const auto& l : _location) + // { + // std::cout << " -- "; + // if (l.lane().has_value()) + // { + // std::cout << "lane[" << *l.lane() << "] "; + // Printer printer; + // const auto& lane = graph.get_lane(*l.lane()); + // if (lane.entry().event()) + // { + // std::cout << " [entry "; + // lane.entry().event()->execute(printer); + // std::cout << "] "; + // } + // const auto& i_wp0 = lane.entry().waypoint_index(); + // const auto& wp0 = graph.get_waypoint(i_wp0); + // const auto& i_wp1 = lane.exit().waypoint_index(); + // const auto& wp1 = graph.get_waypoint(i_wp1); + // std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; + // if (const auto lift = wp0.in_lift()) + // std::cout << "{" << lift->name() << "} "; + // std::cout << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + // if (const auto lift = wp1.in_lift()) + // std::cout << "{" << lift->name() << "} "; + + // if (lane.exit().event()) + // { + // std::cout << "[exit "; + // lane.exit().event()->execute(printer); + // std::cout << "]"; + // } + // std::cout << " | "; + // } + // std::cout << l.waypoint(); + // const auto& wp = graph.get_waypoint(l.waypoint()); + // if (const auto lift = wp.in_lift()) + // { + // std::cout << " {" << lift->name() << "}"; + // } + // if (l.location().has_value()) + // { + // std::cout << " | " << l.location()->transpose(); + // } + // std::cout << std::endl; + // } if (_location.empty()) { 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 1dcf5ce9f..239214c3e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -465,20 +465,20 @@ std::optional ExecutePlan::make( std::function finished, std::optional tail_period) { - std::cout << " --- plan --- " << std::endl; - const auto t0 = plan.get_waypoints().front().time(); - for (const rmf_traffic::agv::Plan::Waypoint& wp : plan.get_waypoints()) - { - std::cout << " -- t=" << rmf_traffic::time::to_seconds(wp.time() - t0) << " "; - if (wp.graph_index().has_value()) - std::cout << "index " << *wp.graph_index() << " "; - std::cout << "[" << wp.position().transpose() << "]" << std::endl; - if (wp.event()) - { - Printer printer; - wp.event()->execute(printer); - } - } + // std::cout << " --- plan --- " << std::endl; + // const auto t0 = plan.get_waypoints().front().time(); + // for (const rmf_traffic::agv::Plan::Waypoint& wp : plan.get_waypoints()) + // { + // std::cout << " -- t=" << rmf_traffic::time::to_seconds(wp.time() - t0) << " "; + // if (wp.graph_index().has_value()) + // std::cout << "index " << *wp.graph_index() << " "; + // std::cout << "[" << wp.position().transpose() << "]" << std::endl; + // if (wp.event()) + // { + // Printer printer; + // wp.event()->execute(printer); + // } + // } std::optional finish_time_estimate; for (const auto& r : plan.get_itinerary()) From 3096fd82049b810baa4242b9ac53cdf5c45fa1ff Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 5 Oct 2023 14:07:06 +0800 Subject: [PATCH 19/89] Fix pybind Signed-off-by: Xiyu Oh --- rmf_fleet_adapter_python/src/graph/graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index c01a7cabf..d65f880de 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -114,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 f28061233d8646696f08d740513994e5be2c5413 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 7 Oct 2023 22:29:13 +0800 Subject: [PATCH 20/89] Fix arrival estimates Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 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 daaec84ed..50c55fdca 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -273,6 +273,7 @@ class EasyFullControl::CommandExecution::Implementation::Data public: std::vector waypoints; std::vector lanes; + std::optional target_location; std::optional final_orientation; rmf_traffic::Duration planned_wait_time; std::optional schedule_override; @@ -462,10 +463,9 @@ class EasyFullControl::CommandExecution::Implementation::Data context->set_lost(Location { now, map, location }); } - if (!waypoints.empty()) + if (target_location.has_value()) { - const auto p_final = - graph.get_waypoint(waypoints.back()).get_location(); + const auto p_final = *target_location; const auto distance = (p_final - p).norm(); double rotation = 0.0; if (final_orientation.has_value()) @@ -1262,17 +1262,19 @@ void EasyCommandHandle::follow_new_path( 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); } @@ -1401,13 +1403,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, From 01152bf98d11833f8a79c1ef2c62139826e08ddf Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 9 Oct 2023 00:32:12 +0800 Subject: [PATCH 21/89] Introduction of dynamic charging concept -- WIP Signed-off-by: Michael X. Grey --- .../agv/FleetUpdateHandle.cpp | 47 +++++++++++++++++-- .../rmf_fleet_adapter/agv/RobotContext.cpp | 24 ++++++++-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 27 ++++++++++- .../agv/internal_FleetUpdateHandle.hpp | 33 +++++++++++-- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 2 +- 5 files changed, 116 insertions(+), 17 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 9f378e6a7..a86a3c1ea 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -653,7 +653,7 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( RCLCPP_INFO( node->get_logger(), - "Assignments updated for robots in fleet [%s] to accommodate task_id [%s]", + "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) @@ -718,7 +718,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 +741,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) @@ -1127,6 +1127,43 @@ void FleetUpdateHandle::Implementation::update_emergency_planner() emergency_config, rmf_traffic::agv::Planner::Options(nullptr)); } +//============================================================================== +void FleetUpdateHandle::Implementation::update_charging_assignments( + const ChargingAssignments& charging) +{ + if (charging.fleet_name != name) + return; + + 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( @@ -1312,7 +1349,7 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const auto FleetUpdateHandle::Implementation::allocate_tasks( rmf_task::ConstRequestPtr new_request, std::vector* errors, - std::optional expectations) const -> std::optional + std::optional expectations) const -> std::optional { // Collate robot states, constraints and combine new requestptr with // requestptr of non-charging tasks in task manager queues @@ -1339,7 +1376,7 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( expect.pending_requests); auto assignments_ptr = std::get_if< - rmf_task::TaskPlanner::Assignments>(&result); + rmf_task::TaskPlanner::TaskAssignments>(&result); if (!assignments_ptr) { 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 6f23a82db..5585b04c9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -657,6 +657,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() { @@ -751,7 +758,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()}); @@ -816,9 +823,9 @@ 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; } //============================================================================== @@ -1016,7 +1023,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)), @@ -1028,6 +1035,7 @@ 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(); @@ -1059,5 +1067,13 @@ void RobotContext::_set_emergency(bool value) } } +//============================================================================== +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{}); +} + } // 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 3cef40712..c92c93f9e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -285,8 +285,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(); @@ -354,7 +358,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; @@ -433,6 +446,10 @@ class RobotContext /// 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); + RobotContext( std::shared_ptr command_handle, std::vector _initial_location, @@ -468,6 +485,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; @@ -480,7 +500,10 @@ 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; 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 dc09596b1..ae0582665 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 @@ -286,8 +287,17 @@ class FleetUpdateHandle::Implementation std::unordered_map emergency_level_for_lift; SharedPlanner emergency_planner; - // Map task id to pair of - using Assignments = rmf_task::TaskPlanner::Assignments; + 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; + + // Map task id to pair of + using TaskAssignments = rmf_task::TaskPlanner::Assignments; using DockParamMap = std::unordered_map< @@ -308,7 +318,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; @@ -556,6 +566,17 @@ class FleetUpdateHandle::Implementation } }; + handle->_pimpl->charging_assignments_sub = + handle->_pimpl->node->create_subscription< + rmf_fleet_msgs::msg::ChargingAssignments>( + "charging_assignments", + 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); @@ -596,14 +617,14 @@ class FleetUpdateHandle::Implementation /// 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( + 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; @@ -616,6 +637,8 @@ class FleetUpdateHandle::Implementation 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; 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..22adf3471 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -54,7 +54,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)); } From 713b57eaaeedb44b250531e24b5654fb998446cb Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 10 Oct 2023 04:54:00 +0000 Subject: [PATCH 22/89] Reworking ChargeBattery request + event Signed-off-by: Michael X. Grey --- .../agv/FleetUpdateHandle.cpp | 27 ++++- .../agv/internal_FleetUpdateHandle.hpp | 2 +- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 101 +++++++++++++++--- .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 8 -- 4 files changed, 114 insertions(+), 24 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 a86a3c1ea..367637220 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1376,7 +1376,7 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( expect.pending_requests); auto assignments_ptr = std::get_if< - rmf_task::TaskPlanner::TaskAssignments>(&result); + rmf_task::TaskPlanner::Assignments>(&result); if (!assignments_ptr) { @@ -1615,6 +1615,31 @@ void FleetUpdateHandle::add_robot( broadcast_client, std::weak_ptr(fleet))}); + 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); + } + // -- Calling the handle_cb should always happen last -- if (handle_cb) { 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 ae0582665..7968d9aaa 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 @@ -574,7 +574,7 @@ class FleetUpdateHandle::Implementation [w = handle->weak_from_this()](const ChargingAssignments& assignments) { if (const auto self = w.lock()) - self->_pimpl->update_charging_assignments(assignments) + self->_pimpl->update_charging_assignments(assignments); }); handle->_pimpl->deserialization.event->add( 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 22adf3471..febd5b7c9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -31,6 +31,93 @@ namespace rmf_fleet_adapter { namespace tasks { +//============================================================================== +// 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 Description : public rmf_task_sequence::Activity::Description + { + public: + Description() + { + // Do nothing + } + + rmf_task_sequence::Activity::ConstModelPtr make_model( + State invariant_initial_state, + const rmf_task::Parameters& parameters) const final + { + + } + + rmf_task::Header generate_header( + const State& initial_state, + const rmf_task::Parameters& parameters) const final + { + + } + }; + + 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 ChargeBatteryEventDescription& 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::make_shared(); + 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 checkpoint, + std::function finished) final + { + + } + + private: + Standby(); + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_traffic::Duration _time_estimate; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state; + ActivePtr _active = nullptr; + } +} + //============================================================================== struct GoToChargerDescription : public rmf_task_sequence::events::Placeholder::Description @@ -149,20 +236,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, 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, From bec3145cbd4297e79d63f7a42500bc776fa51150 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 11 Oct 2023 11:23:08 +0800 Subject: [PATCH 23/89] Making the ChargeBattery event dynamic Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 6 + .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 381 ++++++++++++++++-- 2 files changed, 356 insertions(+), 31 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 5585b04c9..c26d8ce41 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -828,6 +828,12 @@ std::size_t RobotContext::dedicated_charging_wp() const return _charging_wp; } +//============================================================================== +bool RobotContext::waiting_for_charger() const +{ + return _waiting_for_charger; +} + //============================================================================== const rxcpp::observable& RobotContext::observe_battery_soc() const { 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 febd5b7c9..21be0c727 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -31,12 +31,119 @@ 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: @@ -46,17 +153,38 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event } rmf_task_sequence::Activity::ConstModelPtr make_model( - State invariant_initial_state, + 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 State& initial_state, + 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()); + + return rmf_task::Header("Charge Battery", "", duration_estimate); } }; @@ -67,14 +195,14 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event const AssignIDPtr& id, const std::function& get_state, const rmf_task::ConstParametersPtr& parameters, - const ChargeBatteryEventDescription& description, + 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::make_shared(); + auto standby = std::shared_ptr(new Standby); standby->_assign_id = id; standby->_context = context; standby->_time_estimate = header.original_duration_estimate(); @@ -101,22 +229,206 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event } ActivePtr begin( - std::function checkpoint, + std::function, std::function finished) final { - + if (!_active) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning a new charging task for robot [%s]", + _context->requester_id().c_str()); + + _active = Active::make( + _assign_id, + _context, + _state, + _update, + std::move(finished)); + } + + return _active; } private: - Standby(); + Standby() = default; 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( + AssignIDPtr assign_id, + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished) + { + auto active = std::shared_ptr(new Active); + 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_charging_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() = default; + void _consider_restart() + { + const auto charging_wp = _context->dedicated_charging_wp(); + bool location_changed = true; + if (_current_charging_wp.has_value()) + { + if (charging_wp == *_current_charging_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; + } + + _current_charging_wp = charging_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( + [ + charging_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(charging_wp), + update); + }); + + standbys.push_back( + [assign_id = _assign_id, context = _context]( + UpdateFn update) -> StandbyPtr + { + const auto 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); + } + + 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_charging_wp; + std::optional _current_waiting_for_charger; + rmf_task_sequence::Event::ActivePtr _sequence; + }; +}; //============================================================================== struct GoToChargerDescription @@ -245,26 +557,33 @@ void add_charge_battery( { using Bundle = rmf_task_sequence::events::Bundle; using Phase = rmf_task_sequence::phases::SimplePhase; - using ChargeBattery = rmf_task::requests::ChargeBattery; - - auto private_initializer = - std::make_shared(); - - WaitForChargeDescription::add(*private_initializer); - GoToChargerDescription::add(*private_initializer); + using ChargeBatteryTask = rmf_task::requests::ChargeBattery; - 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&) @@ -273,13 +592,13 @@ void add_charge_battery( builder .add_phase( Phase::Description::make( - std::make_shared(), + std::make_shared(), "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, From c4cbe1d6496ca93e895703f6fda50a1b0a7312d7 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 11 Oct 2023 12:04:03 +0800 Subject: [PATCH 24/89] Change finishing task into idle task Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/TaskManager.cpp | 30 +++++++++++++++++++ .../src/rmf_fleet_adapter/TaskManager.hpp | 5 ++++ .../agv/FleetUpdateHandle.cpp | 22 ++++++++++---- .../agv/internal_FleetUpdateHandle.hpp | 1 + 4 files changed, 52 insertions(+), 6 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index f432cc6b1..3dcd9598a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -910,6 +910,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) @@ -1439,6 +1453,22 @@ 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(), + _task_finished(request->booking()->id())), + _context->now()); + return; + } + if (!_responsive_wait_enabled) return; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 1d8759bcc..246dccd92 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; 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 367637220..270aa158f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1609,11 +1609,12 @@ void FleetUpdateHandle::add_robot( if (fleet->_pimpl->broadcast_client) broadcast_client = fleet->_pimpl->broadcast_client; - fleet->_pimpl->task_managers.insert({context, - TaskManager::make( + const auto mgr = TaskManager::make( context, broadcast_client, - std::weak_ptr(fleet))}); + std::weak_ptr(fleet)); + + fleet->_pimpl->task_managers.insert({context, mgr}); const auto c_it = fleet->_pimpl ->unregistered_charging_assignments.find(context->name()); @@ -1640,6 +1641,8 @@ void FleetUpdateHandle::add_robot( 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) { @@ -2265,7 +2268,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 && @@ -2291,15 +2294,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 @@ -2308,7 +2315,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/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 7968d9aaa..b218092dc 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 @@ -260,6 +260,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)); From 77abe9f1907c3feafcf4efc727509f13f4fea502 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 11 Oct 2023 13:46:08 +0000 Subject: [PATCH 25/89] Adding WaitForCancel and introducing a new ParkIndefinitely task Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/ParkRobot.hpp | 62 +++++++++ rmf_fleet_adapter/src/full_control/main.cpp | 4 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 4 +- .../events/WaitForCancel.cpp | 122 ++++++++++++++++++ .../events/WaitForCancel.hpp | 87 +++++++++++++ .../phases/WaitForCharge.cpp | 30 +++-- .../phases/WaitForCharge.hpp | 10 +- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 60 +++++++-- 8 files changed, 348 insertions(+), 31 deletions(-) create mode 100644 rmf_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.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_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.hpp new file mode 100644 index 000000000..054d7f3ab --- /dev/null +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.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__AGV__PARKROBOT_HPP +#define RMF_FLEET_ADAPTER__AGV__PARKROBOT_HPP + +#include + +namespace rmf_fleet_adapter { +namespace agv { + +//============================================================================== +/// 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 agv +} // 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/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index 50c55fdca..c5a248f9b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -2022,8 +2022,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; 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..7126a2e5b --- /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; + }; +}; + +} +} + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP 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..2af040ed3 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); @@ -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,7 @@ 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)) + "]"; StatusMsg initial_msg; initial_msg.status = _description; @@ -105,7 +106,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 +118,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 +143,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 +171,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 +179,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 21be0c727..cec18ea05 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -28,6 +28,8 @@ #include +#include + namespace rmf_fleet_adapter { namespace tasks { @@ -147,11 +149,21 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event class Description : public rmf_task_sequence::Activity::Description { public: - Description() + 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 @@ -184,7 +196,9 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event duration_estimate += estimate_charge_time( initial_soc, recharged_soc, parameters.battery_system()); - return rmf_task::Header("Charge Battery", "", duration_estimate); + std::string name = park ? "Park" : "Charge Battery"; + + return rmf_task::Header(name, "", duration_estimate); } }; @@ -203,6 +217,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event const auto header = description.generate_header(state, *parameters); auto standby = std::shared_ptr(new Standby); + standby->_indefinite = description.indefinite; standby->_assign_id = id; standby->_context = context; standby->_time_estimate = header.original_duration_estimate(); @@ -240,6 +255,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event _context->requester_id().c_str()); _active = Active::make( + _indefinite, _assign_id, _context, _state, @@ -252,6 +268,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event private: Standby() = default; + bool _indefinite; AssignIDPtr _assign_id; agv::RobotContextPtr _context; rmf_traffic::Duration _time_estimate; @@ -265,6 +282,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event { public: static std::shared_ptr make( + bool indefinite, AssignIDPtr assign_id, agv::RobotContextPtr context, rmf_task::events::SimpleEventStatePtr state, @@ -272,6 +290,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event std::function finished) { auto active = std::shared_ptr(new Active); + active->_indefinite = indefinite; active->_assign_id = std::move(assign_id); active->_context = std::move(context); active->_state = std::move(state); @@ -398,11 +417,16 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event }); standbys.push_back( - [assign_id = _assign_id, context = _context]( + [assign_id = _assign_id, context = _context, indefinite = _indefinite]( UpdateFn update) -> StandbyPtr { - const auto recharged_soc = context->task_planner() - ->configuration().constraints().recharge_soc(); + 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(), @@ -418,6 +442,9 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event standbys, _state, _update)->begin([]() {}, _finished); } + std::optional specific_location; + bool _indefinite; + bool _park; AssignIDPtr _assign_id; agv::RobotContextPtr _context; rmf_task::events::SimpleEventStatePtr _state; @@ -491,27 +518,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, @@ -586,13 +623,14 @@ void add_charge_battery( }); 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", ""); From 405fe157120415e5c250496bd1c97f8b36376394 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 12 Oct 2023 11:50:18 +0800 Subject: [PATCH 26/89] Idle tasks runs only once after any active task is finished Signed-off-by: Michael X. Grey --- .../ParkRobotIndefinitely.hpp} | 8 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 8 +- .../src/rmf_fleet_adapter/TaskManager.hpp | 1 + .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 219 +++++++++++++----- 4 files changed, 173 insertions(+), 63 deletions(-) rename rmf_fleet_adapter/include/rmf_fleet_adapter/{agv/ParkRobot.hpp => tasks/ParkRobotIndefinitely.hpp} (93%) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp similarity index 93% rename from rmf_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.hpp rename to rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp index 054d7f3ab..277586bc2 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/ParkRobot.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp @@ -15,13 +15,13 @@ * */ -#ifndef RMF_FLEET_ADAPTER__AGV__PARKROBOT_HPP -#define RMF_FLEET_ADAPTER__AGV__PARKROBOT_HPP +#ifndef RMF_FLEET_ADAPTER__TASKS__PARKROBOT_HPP +#define RMF_FLEET_ADAPTER__TASKS__PARKROBOT_HPP #include namespace rmf_fleet_adapter { -namespace agv { +namespace tasks { //============================================================================== /// Use this task factory to make finisher tasks (idle tasks) that will move the @@ -56,7 +56,7 @@ class ParkRobotIndefinitely : public rmf_task::RequestFactory rmf_utils::impl_ptr _pimpl; }; -} // namespace agv +} // namespace tasks } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__AGV__PARKROBOT_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3dcd9598a..afabcda25 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -1264,7 +1264,7 @@ void TaskManager::_begin_next_task() if (_queue.empty() && _direct_queue.empty()) { - if (!_waiting) + if (!_waiting && !_finished_waiting) _begin_waiting(); return; @@ -1305,6 +1305,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( @@ -1356,7 +1357,7 @@ void TaskManager::_begin_next_task() } else { - if (!_waiting) + if (!_waiting && !_finished_waiting) _begin_waiting(); } @@ -1464,7 +1465,7 @@ void TaskManager::_begin_waiting() _update_cb(), _checkpoint_cb(), _phase_finished_cb(), - _task_finished(request->booking()->id())), + _make_resume_from_waiting()), _context->now()); return; } @@ -1576,6 +1577,7 @@ std::function TaskManager::_make_resume_from_waiting() if (!self) return; + 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 246dccd92..52de08f81 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -332,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/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index cec18ea05..c116771a6 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,8 +28,9 @@ #include -#include +#include +#include namespace rmf_fleet_adapter { namespace tasks { @@ -216,8 +217,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event const auto context = state.get()->value; const auto header = description.generate_header(state, *parameters); - auto standby = std::shared_ptr(new Standby); - standby->_indefinite = description.indefinite; + auto standby = std::shared_ptr(new Standby(description)); standby->_assign_id = id; standby->_context = context; standby->_time_estimate = header.original_duration_estimate(); @@ -249,13 +249,16 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event { if (!_active) { + const std::string& task = _desc.park ? "parking" : "charging"; + RCLCPP_INFO( _context->node()->get_logger(), - "Beginning a new charging task for robot [%s]", + "Beginning a new %s task for robot [%s]", + task.c_str(), _context->requester_id().c_str()); _active = Active::make( - _indefinite, + _desc, _assign_id, _context, _state, @@ -267,8 +270,12 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event } private: - Standby() = default; - bool _indefinite; + Standby(Description desc) + : _desc(desc) + { + // Do nothing + } + Description _desc; AssignIDPtr _assign_id; agv::RobotContextPtr _context; rmf_traffic::Duration _time_estimate; @@ -282,15 +289,14 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event { public: static std::shared_ptr make( - bool indefinite, + 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); - active->_indefinite = indefinite; + 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); @@ -338,7 +344,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event if (_sequence) _sequence->interrupt(std::move(task_is_interrupted)); - _current_charging_wp = std::nullopt; + _current_target_wp = std::nullopt; _current_waiting_for_charger = std::nullopt; return Resume::make( [w = weak_from_this()]() @@ -364,36 +370,50 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event private: - Active() = default; + Active(Description desc) + : _desc(desc) + { + // Do nothing + } void _consider_restart() { - const auto charging_wp = _context->dedicated_charging_wp(); - bool location_changed = true; - if (_current_charging_wp.has_value()) + std::size_t target_wp = _context->dedicated_charging_wp(); + if (!_desc.specific_location.has_value()) { - if (charging_wp == *_current_charging_wp) + bool location_changed = true; + if (_current_target_wp.has_value()) { - location_changed = false; + 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) + 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) { - waiting_changed = false; + // No need to do anything, the charging location has not changed + // nor has the mode changed. + return; } } - - if (!location_changed && !waiting_changed) + else { - // No need to do anything, the charging location has not changed - // nor has the mode changed. - return; + target_wp = _desc.specific_location.value(); + if (_current_target_wp == target_wp) + return; } - _current_charging_wp = charging_wp; + _current_target_wp = target_wp; _current_waiting_for_charger = _context->waiting_for_charger(); using UpdateFn = std::function; @@ -403,7 +423,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event using GoToPlaceDesc = rmf_task_sequence::events::GoToPlace::Description; standbys.push_back( [ - charging_wp, + target_wp, assign_id = _assign_id, context = _context ](UpdateFn update) -> StandbyPtr @@ -412,46 +432,61 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event assign_id, context->make_get_state(), context->task_parameters(), - *GoToPlaceDesc::make(charging_wp), + *GoToPlaceDesc::make(target_wp), update); }); - standbys.push_back( - [assign_id = _assign_id, context = _context, indefinite = _indefinite]( - UpdateFn update) -> StandbyPtr - { - std::optional recharged_soc; - if (!indefinite) + if (_desc.park) + { + standbys.push_back( + [ + assign_id = _assign_id, + context = _context + ](UpdateFn update) -> StandbyPtr { - 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)); - }); + 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); } - std::optional specific_location; - bool _indefinite; - bool _park; + 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_charging_wp; + std::optional _current_target_wp; std::optional _current_waiting_for_charger; rmf_task_sequence::Event::ActivePtr _sequence; }; @@ -462,7 +497,7 @@ 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 } @@ -643,5 +678,77 @@ void add_charge_battery( 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&) 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(); + 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 From c5c35a6cca949b9c69597a5321b86830164af4a2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 12 Oct 2023 12:29:30 +0800 Subject: [PATCH 27/89] Fix some undefined behavior Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 4 +++- .../src/rmf_fleet_adapter/tasks/ChargeBattery.cpp | 8 +++++--- 2 files changed, 8 insertions(+), 4 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 c5a248f9b..87b8420cc 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 @@ -2033,7 +2034,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; 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 c116771a6..da5ea7d8b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -154,7 +154,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event std::optional specific_location_, bool indefinite_, bool park_) - : specific_location(specific_location), + : specific_location(specific_location_), indefinite(indefinite_), park(park_) { @@ -724,7 +724,7 @@ ParkRobotIndefinitely::ParkRobotIndefinitely( //============================================================================== rmf_task::ConstRequestPtr ParkRobotIndefinitely::make_request( - const rmf_task::State&) const + const rmf_task::State& state) const { std::string id = "ParkRobot-" + generate_uuid(); auto phase_desc = std::make_shared( @@ -735,7 +735,9 @@ rmf_task::ConstRequestPtr ParkRobotIndefinitely::make_request( phase_desc), {}) .build("Park", ""); - auto now = _pimpl->time_now_cb(); + 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), From fa3fb33126a525a78839bcc1c75d99fe83e9b247 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 12 Oct 2023 16:49:34 +0800 Subject: [PATCH 28/89] Use global const variable for charging assignments topic name Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp | 2 ++ .../src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp | 2 +- 2 files changed, 3 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 9d0ac0591..435b4f9c2 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -68,6 +68,8 @@ 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"; + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP 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 b218092dc..9973178a1 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 @@ -570,7 +570,7 @@ class FleetUpdateHandle::Implementation handle->_pimpl->charging_assignments_sub = handle->_pimpl->node->create_subscription< rmf_fleet_msgs::msg::ChargingAssignments>( - "charging_assignments", + ChargingAssignmentsTopicName, reliable_transient_qos, [w = handle->weak_from_this()](const ChargingAssignments& assignments) { From e6f3557a80c163eefadfd4de9938ada368c52dc0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 12 Oct 2023 22:43:44 +0800 Subject: [PATCH 29/89] Introduce a charging schedule management node Signed-off-by: Michael X. Grey --- 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 | 238 ++++++++++++++++++ rmf_charging_schedule/setup.cfg | 4 + rmf_charging_schedule/setup.py | 25 ++ 8 files changed, 329 insertions(+) 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 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..cf27f8360 --- /dev/null +++ b/rmf_charging_schedule/rmf_charging_schedule/main.py @@ -0,0 +1,238 @@ +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 + 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', + ], + }, +) From 27e0aea633e170da1386015bab497b3d17a99415 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Sat, 14 Oct 2023 15:14:28 +0800 Subject: [PATCH 30/89] Increase lift localize timeout Signed-off-by: Xiyu Oh --- rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 56081fd7b..43b825c0d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -199,7 +199,7 @@ void RequestLift::ActivePhase::_init_obs() if (me->_context->localize(*me->_localize_after, std::move(cmd))) { me->_rewait_timer = me->_context->node()->try_create_wall_timer( - std::chrono::seconds(30), + std::chrono::seconds(300), [w = me->weak_from_this(), s] { const auto me = w.lock(); From 9bfe603df85b996ab108b2ce3a55acf21c28922b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 14 Oct 2023 16:58:03 +0000 Subject: [PATCH 31/89] Fix cancellation logic for lifts Signed-off-by: Michael X. Grey --- .../src/lift_supervisor/Node.cpp | 11 ++++ .../agv/FleetUpdateHandle.cpp | 32 ++++----- .../rmf_fleet_adapter/agv/RobotContext.cpp | 66 +++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 47 ++++++++++++- .../phases/EndLiftSession.cpp | 41 +----------- .../phases/EndLiftSession.hpp | 2 - .../rmf_fleet_adapter/phases/RequestLift.cpp | 59 ++++++++--------- .../rmf_fleet_adapter/phases/RequestLift.hpp | 2 + 8 files changed, 169 insertions(+), 91 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index fc096dcd0..05b749085 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -92,6 +92,17 @@ void Node::_lift_state_update(LiftState::UniquePtr msg) (lift_request->door_state != msg->door_state)) _lift_request_pub->publish(*lift_request); } + else if (!msg->session_id.empty()) + { + // The lift missed the publication of the session end request so we should + // republish it. + LiftRequest request; + request.lift_name = msg->lift_name; + request.destination_floor = msg->current_floor; + request.session_id = msg->session_id; + 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/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 270aa158f..960010cee 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1503,24 +1503,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->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 - } - ); + 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. 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 c26d8ce41..99646429e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1000,6 +1000,28 @@ void RobotContext::set_localization( _localize = std::move(localization); } +//============================================================================== +std::shared_ptr RobotContext::set_lift_destination( + std::string lift_name, + std::string destination_floor, + bool requested_from_inside) +{ + _lift_destination = std::make_shared( + LiftDestination{ + std::move(lift_name), + std::move(destination_floor), + requested_from_inside + }); + + return _lift_destination; +} + +//============================================================================== +void RobotContext::release_lift() +{ + _lift_destination = nullptr; +} + //============================================================================== RobotContext::RobotContext( std::shared_ptr command_handle, @@ -1081,5 +1103,49 @@ void RobotContext::_set_charging(std::size_t wp, bool waiting_for_charger) _charging_change_publisher.get_subscriber().on_next(Empty{}); } +//============================================================================== +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. + _lift_destination = nullptr; + } + } + + 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) + { + 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); +} + } // 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 c92c93f9e..840d4535e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -37,6 +37,7 @@ #include #include +#include #include @@ -163,6 +164,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 @@ -175,6 +177,14 @@ struct Lost std::unique_ptr ticket; }; +//============================================================================== +struct LiftDestination +{ + std::string lift_name; + std::string destination_floor; + bool requested_from_inside; +}; + //============================================================================== class RobotContext : public std::enable_shared_from_this, @@ -434,6 +444,15 @@ class RobotContext /// Set the callback for localizing the robot void set_localization(EasyFullControl::LocalizationRequest localization); + /// 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(); + /// 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); @@ -450,6 +469,28 @@ class RobotContext /// charger change notification. void _set_charging(std::size_t wp, bool waiting_for_charger); + 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); + }); + + return context; + } + +private: + RobotContext( std::shared_ptr command_handle, std::vector _initial_location, @@ -465,8 +506,6 @@ 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; @@ -525,6 +564,10 @@ class RobotContext Reporting _reporting; /// Keep track of a lost robot std::optional _lost; + + void _check_lift_state(const rmf_lift_msgs::msg::LiftState& state); + std::shared_ptr _lift_destination; + rmf_rxcpp::subscription_guard _lift_subscription; }; using RobotContextPtr = std::shared_ptr; 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..fa2f414dd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp @@ -46,6 +46,7 @@ EndLiftSession::Active::Active( _destination(std::move(destination)) { _description = "Ending session with lift [" + lift_name + "]"; + _context->release_lift(); } //============================================================================== @@ -86,24 +87,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 +107,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()); } //============================================================================== 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/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index 56081fd7b..5651702e8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -71,7 +71,7 @@ void RequestLift::ActivePhase::emergency_alarm(bool /*on*/) //============================================================================== void RequestLift::ActivePhase::cancel() { - _cancelled.get_subscriber().on_next(true); + // GoToPlace / ExecutePlan don't call the cancel function anyway } //============================================================================== @@ -179,12 +179,15 @@ void RequestLift::ActivePhase::_init_obs() if (me->_localize_after.has_value()) { - auto finish = [s, worker = me->_context->worker()]() + auto finish = [s, worker = me->_context->worker(), weak]() { - worker.schedule([s](const auto&) - { - s.on_completed(); - }); + worker.schedule([s, weak](const auto&) + { + if (const auto me = weak.lock()) + me->_finish(); + + s.on_completed(); + }); }; auto cmd = agv::EasyFullControl ::CommandExecution::Implementation::make_hold( @@ -200,9 +203,9 @@ void RequestLift::ActivePhase::_init_obs() { me->_rewait_timer = me->_context->node()->try_create_wall_timer( std::chrono::seconds(30), - [w = me->weak_from_this(), s] + [weak, s] { - const auto me = w.lock(); + const auto me = weak.lock(); if (!me) return; @@ -214,26 +217,15 @@ void RequestLift::ActivePhase::_init_obs() "process is finished.", me->_context->requester_id().c_str()); + me->_finish(); s.on_completed(); }); return; } } - if (!me->_cancelled.get_value() || me->_located == Located::Inside) - { - s.on_completed(); - } - else if (me->_located == Located::Outside) - { - auto transport = me->_context->node(); - me->_lift_end_phase = EndLiftSession::Active::make( - me->_context, - me->_lift_name, - me->_destination); - - me->_lift_end_phase->observe().subscribe(s); - } + me->_finish(); + s.on_completed(); })); } @@ -356,15 +348,22 @@ 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, _located == Located::Inside); + } +} - _context->node()->lift_request()->publish(msg); +//============================================================================== +void RequestLift::ActivePhase::_finish() +{ + if (_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); + } } //============================================================================== 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 0b6f34822..a117a0e33 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -74,6 +74,7 @@ struct RequestLift rmf_traffic::PlanId _plan_id; std::optional _localize_after; rmf_rxcpp::subscription_guard _reset_session_subscription; + std::shared_ptr _destination_handle; struct WatchdogInfo { @@ -100,6 +101,7 @@ struct RequestLift const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state); void _do_publish(); + void _finish(); }; class PendingPhase : public LegacyTask::PendingPhase From cc9b921c1eb7f0bdfcf9f621d7881cdafc0a2861 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 17 Oct 2023 19:43:10 +0000 Subject: [PATCH 32/89] Adding option to print position update debug info Signed-off-by: Michael X. Grey --- .../agv/RobotUpdateHandle.hpp | 3 + .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 34 ++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 35 +++--- .../rmf_fleet_adapter/agv/RobotContext.hpp | 106 ++++++++++++++++++ .../agv/RobotUpdateHandle.cpp | 83 ++++++++++---- .../agv/internal_RobotUpdateHandle.hpp | 17 +-- rmf_fleet_adapter_python/src/adapter.cpp | 6 + 7 files changed, 230 insertions(+), 54 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 2b3138c3e..bb6953484 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -475,6 +475,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/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index 87b8420cc..4e57bdf5d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -457,10 +457,36 @@ class EasyFullControl::CommandExecution::Implementation::Data 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(starts); } else { + if (context->debug_positions) + { + 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) + { + ss << "\n -- " << print_waypoint(wp, graph); + } + ss << lanes.size() << " lanes:"; + for (std::size_t lane : lanes) + { + ss << "\n -- " << print_lane(lane, graph); + } + + std::cout << ss.str() << std::endl; + } context->set_lost(Location { now, map, location }); } @@ -616,6 +642,10 @@ auto EasyFullControl::CommandExecution::Implementation::make_hold( 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); } }; @@ -1585,6 +1615,10 @@ void EasyFullControl::EasyRobotUpdateHandle::update( } } + if (context->debug_positions) + { + std::cout << "Searching for location from " << __FILE__ << "|" << __LINE__ << std::endl; + } updater->nav_params->search_for_location(state.map(), position, *context); }); } 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 c26d8ce41..e91990b55 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -48,10 +48,23 @@ void NavParams::search_for_location( 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 }); } } @@ -345,24 +358,6 @@ const rmf_traffic::agv::Plan::StartSet& RobotContext::location() const return _location; } -class Printer : public rmf_traffic::agv::Graph::Lane::Executor -{ -public: - Printer() - { - // Do nothing - } - - void execute(const DoorOpen&) override { std::cout << __LINE__; } - void execute(const DoorClose&) override { std::cout << __LINE__; } - void execute(const LiftSessionBegin&) override { std::cout << __LINE__; } - void execute(const LiftDoorOpen&) override { std::cout << __LINE__; } - void execute(const LiftSessionEnd&) override { std::cout << __LINE__; } - void execute(const LiftMove&) override { std::cout << __LINE__; } - void execute(const Wait&) override { std::cout << __LINE__; } - void execute(const Dock& dock) override { std::cout << __LINE__; } -}; - //============================================================================== void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) { @@ -423,6 +418,10 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) if (_location.empty()) { + if (debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST" << std::endl; + } set_lost(std::nullopt); return; } 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 c92c93f9e..568ecb2ca 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -59,6 +59,110 @@ using SharedPlanner = std::shared_ptr< using Destination = EasyFullControl::Destination; using VertexStack = std::shared_ptr>; +//============================================================================== +class EventPrinter : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + EventPrinter() + { + // Do nothing + } + + void execute(const DoorOpen&) override { text = "DoorOpen"; } + void execute(const DoorClose&) override { text = "DoorClose"; } + void execute(const LiftSessionBegin&) override { text = "LiftSessionBegin"; } + void execute(const LiftDoorOpen&) override { text = "LiftDoorOpen"; } + void execute(const LiftSessionEnd&) override { text = "LiftSessionEnd"; } + void execute(const LiftMove&) override { text = "LiftMove"; } + void execute(const Wait&) override { text = "Wait"; } + void execute(const Dock& dock) override { text = "Dock"; } + + 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_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 << " | exact location <" << l.location()->transpose() << ">"; + } + else + { + ss << " | exactly on waypoint"; + } + + ss << " | orientation " << l.orientation() * 180.0 / M_PI; + } + + return ss.str(); +} + //============================================================================== struct NavParams { @@ -465,6 +569,8 @@ class RobotContext rmf_task::State state, std::shared_ptr task_planner); + bool debug_positions = false; + private: std::weak_ptr _command_handle; 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 ba186d0f0..319c5109d 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)); }); } } @@ -169,6 +192,12 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [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 +206,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 +227,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); }); } @@ -822,6 +865,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) @@ -1099,26 +1153,11 @@ 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/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index 25c3d54e9..02baca36f 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 @@ -193,22 +193,11 @@ 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_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 620a3ec8b..41b1bee89 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -246,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")) From abef0ca1868dd0ed4ec95bc60a851f7dccb1109c Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 19 Oct 2023 04:39:50 +0800 Subject: [PATCH 33/89] Change QoS for adapter_lift_requests and lift_requests to transient local Signed-off-by: Xiyu Oh --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 5 +++-- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index fc096dcd0..23dadc48b 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -28,12 +28,13 @@ Node::Node() : rclcpp::Node("rmf_lift_supervisor") { const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto transient_qos = rclcpp::SystemDefaultsQoS().reliable().keep_last(10).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)); 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..4dcb18dfd 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,7 @@ std::shared_ptr Node::make( auto default_qos = rclcpp::SystemDefaultsQoS(); default_qos.keep_last(100); + auto transient_qos = rclcpp::SystemDefaultsQoS().reliable().keep_last(10).transient_local(); node->_door_state_obs = node->create_observable( @@ -55,7 +56,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( From 4712243f54702cc3a958aaf82725f4b580e341ef Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 19 Oct 2023 14:40:13 +0000 Subject: [PATCH 34/89] Keep last 100 Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 3 ++- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 23dadc48b..87d2025b7 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -28,7 +28,8 @@ Node::Node() : rclcpp::Node("rmf_lift_supervisor") { const auto default_qos = rclcpp::SystemDefaultsQoS(); - const auto transient_qos = rclcpp::SystemDefaultsQoS().reliable().keep_last(10).transient_local(); + const auto transient_qos = rclcpp::SystemDefaultsQoS() + .reliable().keep_last(100).transient_local(); _lift_request_pub = create_publisher( FinalLiftRequestTopicName, transient_qos); 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 4dcb18dfd..e4ee34c8a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -36,7 +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(10).transient_local(); + auto transient_qos = rclcpp::SystemDefaultsQoS() + .reliable().keep_last(100).transient_local(); node->_door_state_obs = node->create_observable( From 1e4b479aa4921c2be675ede9cfec34285a375bc1 Mon Sep 17 00:00:00 2001 From: Xiyu Date: Fri, 20 Oct 2023 04:03:35 +0800 Subject: [PATCH 35/89] Change QoS for adapter_lift_requests and lift_requests to transient local (#306) Signed-off-by: Xiyu Oh Signed-off-by: Michael X. Grey Co-authored-by: Michael X. Grey --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 6 ++++-- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp | 4 +++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index fc096dcd0..87d2025b7 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)); 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..e4ee34c8a 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( From 01fcb238aa595e16ed5986988b9077b420215b19 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 24 Oct 2023 02:07:30 +0800 Subject: [PATCH 36/89] Add debug logging to lift supervisor Signed-off-by: Xiyu Oh --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 87d2025b7..395e9f4af 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -55,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]", + msg->session_id.c_str(), msg->destination_floor.c_str() + ); auto& curr_request = _active_sessions.insert( std::make_pair(msg->lift_name, nullptr)).first->second; @@ -67,6 +73,11 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) else { _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; } } @@ -93,6 +104,11 @@ 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); + 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() + ); } // For now, we do not need to publish this. From 5c503762e3b42e57faf139efd56e408f3db280a6 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 26 Oct 2023 04:11:57 +0000 Subject: [PATCH 37/89] Adding a mutex group manager Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/CMakeLists.txt | 20 +++ .../rmf_fleet_adapter/StandardNames.hpp | 3 + .../src/mutex_group_supervisor/main.cpp | 163 ++++++++++++++++++ 3 files changed, 186 insertions(+) create mode 100644 rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp 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 435b4f9c2..c3a94086c 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -70,6 +70,9 @@ 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"; + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP 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..db5185276 --- /dev/null +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -0,0 +1,163 @@ +/* + * 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; + +class Node : 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.claimer); + 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); + } + } + } + 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.claimer, timestamps); + for (const auto& s : latest_states.assignments) + { + if (s.group == request.group && !s.claimed.empty()) + { + // The group is already claimed, so nothing to be done here + return; + } + } + pick_next(request.group); + } + + void do_heartbeat() + { + state_pub->publish(latest_states); + } + + void pick_next(const std::string& group) + { + const auto& claimers = mutex_groups[group]; + std::optional> earliest; + for (const auto& [claimer, timestamp] : claimers) + { + const auto& t = timestamp.claim_time; + if (!earliest.has_value() || t < earliest->first) + { + earliest = std::make_pair(t, claimer); + } + } + + std::string claimer; + if (earliest.has_value()) + { + claimer = earliest->second; + } + bool group_found = false; + for (auto& a : latest_states.assignments) + { + if (a.group == group) + { + a.claimed = claimer; + group_found = true; + break; + } + } + if (!group_found) + { + latest_states.assignments.push_back( + rmf_fleet_msgs::build() + .group(group) + .claimed(claimer)); + } + + state_pub->publish(latest_states); + } + + 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[]) +{ + +} From d2e5dfa951c5d84f75b0d471592def031dfa6140 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 26 Oct 2023 13:38:44 +0000 Subject: [PATCH 38/89] Finished implementation of mutex group supervisor -- needs testing Signed-off-by: Michael X. Grey --- .../src/mutex_group_supervisor/main.cpp | 44 ++++++++++++++++++- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index db5185276..1dfe179cc 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -85,6 +85,7 @@ class Node : rclcpp::Node { g_it->second.erase(c_it); pick_next(request.group); + state_pub->publish(latest_states); } } } @@ -108,6 +109,47 @@ class Node : rclcpp::Node 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& [claimer, timestamp] : claims) + { + if (timestamp.heartbeat_time + timeout < now) + { + remove_claims.push_back(claimer); + } + } + + std::string current_claimer; + for (const auto& assignment : latest_states.assignments) + { + if (assignment.group == group) + { + current_claimer = assignment.claimed; + 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); } @@ -146,8 +188,6 @@ class Node : rclcpp::Node .group(group) .claimed(claimer)); } - - state_pub->publish(latest_states); } std::unordered_map mutex_groups; From ee8d62574fcd0e830072aa85d85c250753b1844d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 26 Oct 2023 23:22:59 +0000 Subject: [PATCH 39/89] Adding data flows for mutex groups Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/Node.cpp | 20 +++++ .../src/rmf_fleet_adapter/agv/Node.hpp | 12 +++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 7 ++ .../events/LockMutexGroup.cpp | 27 ++++++ .../events/LockMutexGroup.hpp | 88 +++++++++++++++++++ .../events/WaitForCancel.hpp | 4 +- 6 files changed, 156 insertions(+), 2 deletions(-) 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 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 e4ee34c8a..000da0819 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -108,6 +108,14 @@ std::shared_ptr Node::make( node->create_publisher( TaskApiResponses, transient_local_qos); + node->_mutex_group_states_obs = + node->create_observable( + MutexGroupStatesTopicName, transient_local_qos); + + node->_mutex_group_request_pub = + node->create_publisher( + MutexGroupRequestTopicName, transient_local_qos); + return node; } @@ -231,5 +239,17 @@ 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_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..26be6c875 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,14 @@ 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 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 +189,8 @@ 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_states_obs; }; } // namespace agv 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 568ecb2ca..7e386b077 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -538,6 +538,9 @@ class RobotContext /// Set the callback for localizing the robot void set_localization(EasyFullControl::LocalizationRequest localization); + /// Set the mutex group that this robot needs to have locked. + void set_mutex_group(std::string group); + /// 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); @@ -631,6 +634,10 @@ class RobotContext Reporting _reporting; /// Keep track of a lost robot std::optional _lost; + + std::string _mutex_group; + rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; + rmf_rxcpp::subscription_guard _mutex_group_sanity_check; }; using RobotContextPtr = std::shared_ptr; 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..ab5b56000 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -0,0 +1,27 @@ +/* + * 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" + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== + + +} // 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..e5c32a6d3 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -0,0 +1,88 @@ +/* + * 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 + + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class LockMutexGroup : 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, + std::string mutex_group); + + 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; + std::string _mutex_group; + }; + + 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__LOCKMUTEXGROUP_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp index 7126a2e5b..9c4aad415 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp @@ -81,7 +81,7 @@ class WaitForCancel : public rmf_task_sequence::Event }; }; -} -} +} // namespace events +} // namespace rmf_fleet_adapter #endif // SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP From 213198235528541182721260e1efadeeca64604a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 26 Oct 2023 23:58:08 +0000 Subject: [PATCH 40/89] Add context plumbing for mutex groups Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 65 +++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 25 +++++++ 2 files changed, 90 insertions(+) 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 51d616a16..c7741e1a2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1021,6 +1021,34 @@ void RobotContext::release_lift() _lift_destination = nullptr; } +//============================================================================== +void RobotContext::set_mutex_group(std::string group) +{ + if (group.is_empty()) + { + release_mutex_group(); + return; + } + + _mutex_group_claim_time = _node->now(); + _mutex_group = std::move(group); + _publish_mutex_group_request(); +} + +//============================================================================== +void RobotContext::release_mutex_group() +{ + if (_mutex_group.empty()) + return; + + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(_mutex_group) + .claimer(requester_id()) + .claim_time(_mutex_group_claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); +} + //============================================================================== RobotContext::RobotContext( std::shared_ptr command_handle, @@ -1146,5 +1174,42 @@ void RobotContext::_check_lift_state( _node->lift_request()->publish(msg); } +//============================================================================== +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.claimed != requester_id()) + return; + + if (assignment.group == _mutex_group) + return; + + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(assignment.group) + .claimer(requester_id()) + .claim_time(_mutex_group_claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + } +} + +//============================================================================== +void RobotContext::_publish_mutex_group_request() +{ + if (!_mutex_group.empty()) + { + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(_mutex_group) + .claimer(requester_id()) + .claim_time(_mutex_group_claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); + } +} + } // 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 e191415a1..4b78ece1b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -593,6 +593,28 @@ class RobotContext self->_check_lift_state(*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(); + }); + + 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_request(); + }); + return context; } @@ -678,7 +700,10 @@ class RobotContext std::shared_ptr _lift_destination; rmf_rxcpp::subscription_guard _lift_subscription; + void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); + void _publish_mutex_group_request(); std::string _mutex_group; + builtin_interfaces::msg::Time _mutex_group_claim_time; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; }; From b498b3aad80a1f35e7d233da21f2758aafefa7f4 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 30 Oct 2023 03:22:13 +0000 Subject: [PATCH 41/89] Implement LockMutexGroup event -- need to update ExecutePlan Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/TaskManager.cpp | 3 + .../rmf_fleet_adapter/agv/RobotContext.cpp | 31 ++++- .../rmf_fleet_adapter/agv/RobotContext.hpp | 6 +- .../events/LockMutexGroup.cpp | 130 ++++++++++++++++++ .../events/LockMutexGroup.hpp | 5 +- 5 files changed, 172 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 afabcda25..ee8215ba3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -1467,6 +1467,7 @@ void TaskManager::_begin_waiting() _phase_finished_cb(), _make_resume_from_waiting()), _context->now()); + _context->current_task_id(request->booking()->id()); return; } @@ -1513,6 +1514,7 @@ void TaskManager::_begin_waiting() _update_cb(), _make_resume_from_waiting()), _context->now()); + _context->current_task_id(task_id); } //============================================================================== @@ -2165,6 +2167,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/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index c7741e1a2..2c16b67c2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1024,12 +1024,17 @@ void RobotContext::release_lift() //============================================================================== void RobotContext::set_mutex_group(std::string group) { - if (group.is_empty()) + if (group.empty()) { release_mutex_group(); return; } + if (group == _mutex_group) + { + return; + } + _mutex_group_claim_time = _node->now(); _mutex_group = std::move(group); _publish_mutex_group_request(); @@ -1200,6 +1205,30 @@ void RobotContext::_check_mutex_groups( //============================================================================== void RobotContext::_publish_mutex_group_request() { + 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 (!_mutex_group.empty()) + { + 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.", + _mutex_group.c_str(), + requester_id().c_str()); + _mutex_group.clear(); + } + } + } + if (!_mutex_group.empty()) { _node->mutex_group_request()->publish( 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 4b78ece1b..30e19ebce 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -560,6 +560,9 @@ class RobotContext /// Set the mutex group that this robot needs to have locked. void set_mutex_group(std::string group); + /// If we are holding a mutex group, release it + void release_mutex_group(); + /// 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); @@ -601,7 +604,7 @@ class RobotContext if (!self) return; - self->_check_mutex_groups(); + self->_check_mutex_groups(*msg); }); context->_mutex_group_heartbeat = context->_node->try_create_wall_timer( @@ -706,6 +709,7 @@ class RobotContext builtin_interfaces::msg::Time _mutex_group_claim_time; 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/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index ab5b56000..869f71aae 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -21,7 +21,137 @@ namespace rmf_fleet_adapter { namespace events { //============================================================================== +auto LockMutexGroup::Standby::make( + agv::RobotContextPtr context, + const AssignIDPtr& id, + std::string mutex_group) +-> std::shared_ptr +{ + auto standby = std::shared_ptr(new Standby); + standby->_context = std::move(context); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + "Lock mutex group [" + mutex_group + "]", + "Waiting for the mutex group to be locked", + rmf_task::Event::Status::Standby, {}, standby->_context->clock()); + standby->_mutex_group = std::move(mutex_group); + 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), _mutex_group); +} + +//============================================================================== +auto LockMutexGroup::Active::make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + std::string mutex_group) -> std::shared_ptr +{ + auto active = std::shared_ptr(new Active); + active->_context = std::move(context); + active->_state = std::move(state); + active->_finished = std::move(finished); + active->_mutex_group = std::move(mutex_group); + active->_state->update_status(Status::Underway); + + using MutexGroupStatesPtr = + std::shared_ptr; + + active->_listener = active->_context->node()->mutex_group_states() + .observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe( + [weak = active->weak_from_this()]( + const MutexGroupStatesPtr& mutex_group_states) + { + const auto self = weak.lock(); + if (!self) + return; + + for (const auto& assignment : mutex_group_states->assignments) + { + if (assignment.group == self->_mutex_group) + { + if (assignment.claimed == self->_context->requester_id()) + { + const auto finished = self->_finished; + self->_finished = nullptr; + if (finished) + { + finished(); + return; + } + } + } + } + }); + + active->_context->set_mutex_group(active->_mutex_group); + + 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); + _context->worker().schedule([finished = _finished](const auto&) + { + finished(); + }); +} + +//============================================================================== +void LockMutexGroup::Active::kill() +{ + cancel(); +} } // 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 index e5c32a6d3..f82ec16e2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -61,7 +61,8 @@ class LockMutexGroup : public rmf_task_sequence::Event static std::shared_ptr make( agv::RobotContextPtr context, rmf_task::events::SimpleEventStatePtr state, - std::function finished); + std::function finished, + std::string mutex_group); ConstStatePtr state() const final; @@ -79,6 +80,8 @@ class LockMutexGroup : public rmf_task_sequence::Event agv::RobotContextPtr _context; rmf_task::events::SimpleEventStatePtr _state; std::function _finished; + std::string _mutex_group; + rmf_rxcpp::subscription_guard _listener; }; }; From eb242f3e20cfe9ce5259a83d369820ffa5eda9c3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 31 Oct 2023 03:45:06 +0000 Subject: [PATCH 42/89] Add mutex group locking and unlocking to ExecutePlan and MoveRobot respectively Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 6 ++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 3 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 96 ++++++++++++++++--- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 50 +++++++++- 4 files changed, 138 insertions(+), 17 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 2c16b67c2..445630620 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1021,6 +1021,12 @@ void RobotContext::release_lift() _lift_destination = nullptr; } +//============================================================================== +const std::string& RobotContext::current_mutex_group() const +{ + return _mutex_group; +} + //============================================================================== void RobotContext::set_mutex_group(std::string group) { 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 30e19ebce..8dc7003b4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -557,6 +557,9 @@ class RobotContext /// Indicate that the lift is no longer needed void release_lift(); + /// What mutex group is currently being locked. + const std::string& current_mutex_group() const; + /// Set the mutex group that this robot needs to have locked. void set_mutex_group(std::string group); 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 239214c3e..45f0a7860 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" @@ -45,10 +46,12 @@ struct LegacyPhaseWrapper LegacyPhaseWrapper( std::shared_ptr phase_, rmf_traffic::Time time_, - rmf_traffic::Dependencies dependencies_) + rmf_traffic::Dependencies dependencies_, + std::string 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 } @@ -56,6 +59,7 @@ struct LegacyPhaseWrapper std::shared_ptr phase; rmf_traffic::Time time; rmf_traffic::Dependencies dependencies; + std::string mutex_group_dependency; }; using LegacyPhases = std::vector; @@ -113,7 +117,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _phases.emplace_back( std::make_shared( _context, dock.dock_name()), - _event_start_time, waypoint.dependencies()); + _event_start_time, waypoint.dependencies(), ""); _continuous = false; } @@ -127,7 +131,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.name(), _context->requester_id(), _event_start_time + open.duration()), - _event_start_time, waypoint.dependencies()); + _event_start_time, waypoint.dependencies(), ""); _continuous = true; } @@ -142,7 +146,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, waypoint.dependencies()); + _event_start_time, waypoint.dependencies(), ""); _continuous = true; } @@ -158,7 +162,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _event_start_time, phases::RequestLift::Located::Outside, _plan_id), - _event_start_time, waypoint.dependencies()); + _event_start_time, waypoint.dependencies(), ""); _continuous = true; } @@ -193,7 +197,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor phases::RequestLift::Located::Inside, _plan_id, localize), - _event_start_time, waypoint.dependencies()); + _event_start_time, waypoint.dependencies(), ""); _moving_lift = false; _continuous = true; @@ -208,7 +212,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, waypoint.dependencies()); + _event_start_time, waypoint.dependencies(), ""); _continuous = true; } @@ -480,6 +484,8 @@ std::optional ExecutePlan::make( // } // } + const auto& graph = context->navigation_graph(); + std::optional finish_time_estimate; for (const auto& r : plan.get_itinerary()) { @@ -499,6 +505,7 @@ std::optional ExecutePlan::make( plan.get_waypoints(); std::vector move_through; + std::string current_mutex_group; LegacyPhases legacy_phases; while (!waypoints.empty()) @@ -507,6 +514,55 @@ std::optional ExecutePlan::make( bool event_occurred = false; for (; it != waypoints.end(); ++it) { + std::string new_mutex_group; + if (it->graph_index().has_value()) + { + const auto& group = + graph.get_waypoint(*it->graph_index()).in_mutex_group(); + if (!group.empty()) + { + new_mutex_group = group; + } + } + + for (const auto l : it->approach_lanes()) + { + const auto& lane = graph.get_lane(l); + const auto& group = lane.properties().in_mutex_group(); + if (!group.empty()) + { + new_mutex_group = group; + break; + } + } + + std::string mutex_group_dependency; + if (new_mutex_group != current_mutex_group) + { + if (!new_mutex_group.empty()) + { + if (move_through.size() > 1) + { + legacy_phases.emplace_back( + std::make_shared( + context, move_through, plan_id, tail_period), + move_through.back().time(), move_through.back().dependencies(), + current_mutex_group); + + move_through.clear(); + waypoints.erase(waypoints.begin(), it); + current_mutex_group = std::move(new_mutex_group); + + // We treat this the same as an event occurring to indicate that + // we should keep looping. + event_occurred = true; + break; + } + } + + current_mutex_group = std::move(new_mutex_group); + } + move_through.push_back(*it); if (it->event()) @@ -516,7 +572,7 @@ 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(), mutex_group_dependency); } move_through.clear(); @@ -568,11 +624,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_group); } else { - legacy_phases.emplace_back(nullptr, it->time(), it->dependencies()); + legacy_phases.emplace_back( + nullptr, it->time(), it->dependencies(), current_mutex_group); } // Have the next sequence of waypoints begin with this one. @@ -596,7 +653,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_group); } if (!event_occurred) @@ -631,6 +689,20 @@ std::optional ExecutePlan::make( } else { + if (!head->mutex_group_dependency.empty()) + { + standbys.push_back( + [ + context, + event_id, + mutex_group = head->mutex_group_dependency + ](UpdateFn update) + { + return LockMutexGroup::Standby::make( + context, event_id, mutex_group); + }); + } + if (head->phase) { standbys.push_back( 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..3d778f2ef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -247,17 +247,24 @@ void MoveRobot::Action::operator()(const Subscriber& s) 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, + w = action->weak_from_this(), now, new_cumulative_delay ](const auto&) { + 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) { @@ -265,13 +272,36 @@ void MoveRobot::Action::operator()(const Subscriber& s) const auto t_it = traj.find(now); if (t_it != traj.end() && t_it != traj.begin()) { + std::size_t index = t_it->index() - 1; if (t_it->time() == now) { - context->itinerary().reached(plan_id, i, t_it->index()); + index = t_it->index(); } - else + + context->itinerary().reached(plan_id, i, index); + } + } + + if (!context->current_mutex_group().empty()) + { + const auto adjusted_now = now + new_cumulative_delay; + const auto& graph = context->navigation_graph(); + for (const auto& wp : self->_waypoints) + { + if (wp.time() > adjusted_now) { - context->itinerary().reached(plan_id, i, t_it->index() - 1); + break; + } + + if (wp.graph_index().has_value()) + { + const auto& g = graph.get_waypoint(*wp.graph_index()) + .in_mutex_group(); + if (g.empty()) + { + context->release_mutex_group(); + break; + } } } } @@ -288,6 +318,16 @@ void MoveRobot::Action::operator()(const Subscriber& s) 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(); + if (graph.get_waypoint(*last_index).in_mutex_group().empty()) + { + self->_context->release_mutex_group(); + } + } } LegacyTask::StatusMsg msg; From e8359e1ab17f3113d632e0686664fb93e214a006 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 31 Oct 2023 20:39:13 +0000 Subject: [PATCH 43/89] Release lift when a replan doesn't need the lift anymore Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 6 ++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 ++ .../rmf_fleet_adapter/events/GoToPlace.cpp | 64 +++++++++++++++++++ 3 files changed, 74 insertions(+) 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 99646429e..33c085707 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1000,6 +1000,12 @@ void RobotContext::set_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, 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 840d4535e..39bd51076 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -444,6 +444,9 @@ class RobotContext /// 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, @@ -547,6 +550,7 @@ class RobotContext rxcpp::observable _battery_soc_obs; rmf_task::State _current_task_end_state; std::optional _current_task_id; + std::mutex _current_task_id_mutex; std::shared_ptr _task_planner; std::weak_ptr _task_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 c3b465641..cdfb4af34 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -483,6 +483,45 @@ void GoToPlace::Active::_schedule_retry() }); } +//============================================================================== +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; + + 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 (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; + } +}; + //============================================================================== void GoToPlace::Active::_execute_plan( const rmf_traffic::PlanId plan_id, @@ -507,6 +546,31 @@ void GoToPlace::Active::_execute_plan( _goal.waypoint(), _context->requester_id().c_str()); + 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) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is releasing lift [%s] after a replan because it is no " + "longer needed.", + _context->requester_id().c_str(), + current_lift->lift_name.c_str()); + _context->release_lift(); + } + } + _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); From 9514a887c6f3d1e56f7ba29460190636f92284bd Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 31 Oct 2023 21:03:07 +0000 Subject: [PATCH 44/89] Fix mutex for current task ID Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 16 ++++++---------- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 4 +++- 2 files changed, 9 insertions(+), 11 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 e91990b55..ea3cead67 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -777,23 +777,19 @@ 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); return *this; } //============================================================================== -const std::string RobotContext::copy_current_task_id() const +std::string RobotContext::copy_current_task_id() const { - std::mutex _mutex; - std::unique_lock lock(); - - { - std::unique_lock lock(_mutex); - if (_current_task_id.has_value()) - return _current_task_id.value(); + std::unique_lock lock(*_current_task_id_mutex); + if (_current_task_id.has_value()) + return _current_task_id.value(); - return {}; - } + return {}; } //============================================================================== 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 568ecb2ca..baf4545c2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -453,7 +453,7 @@ class RobotContext /// Get a string copy of the current task ID of the robot, or an empty string /// if the robot is not performing any task - const std::string copy_current_task_id() const; + std::string copy_current_task_id() const; /// Get the current battery state of charge double current_battery_soc() const; @@ -614,6 +614,8 @@ class RobotContext 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; From 0018c0d0f36f4e52b10509acb0781fc7542010b8 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 1 Nov 2023 02:33:57 +0000 Subject: [PATCH 45/89] Identify when an off-vertex location is inside of a lift Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) 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 4e57bdf5d..1d9677600 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1284,6 +1284,17 @@ void EasyCommandHandle::follow_new_path( { in_lift = graph.get_waypoint(*wp1.graph_index()).in_lift(); } + else + { + for (const auto& lift : graph.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( From 45fbd371c1f43940ed7d9dbcf85599dea15015ee Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 3 Nov 2023 14:32:39 +0800 Subject: [PATCH 46/89] Add shared ptr to ActionExecution pybind Signed-off-by: Xiyu Oh --- rmf_fleet_adapter_python/src/adapter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 41b1bee89..2060e2622 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -302,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, From bd0d89b4fc8be2cf7cd120f7f9e639afcacb7ef1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 6 Nov 2023 21:45:27 +0000 Subject: [PATCH 47/89] Release lift from robots that are idle outside of a lift Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 50 +++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 2 + 2 files changed, 52 insertions(+) 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 56821ad23..e09d2e4a4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -779,6 +779,7 @@ 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; } @@ -1013,6 +1014,7 @@ std::shared_ptr RobotContext::set_lift_destination( std::move(destination_floor), requested_from_inside }); + _initial_time_idle_outside_lift = std::nullopt; return _lift_destination; } @@ -1117,6 +1119,54 @@ void RobotContext::_check_lift_state( // lift usage was cancelled while the robot was outside of the lift. // Therefore we should release the usage of the lift. _lift_destination = nullptr; + _initial_time_idle_outside_lift = std::nullopt; + } + 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& known_lifts = graph.known_lifts(); + const auto l_it = std::find_if( + known_lifts.begin(), + known_lifts.end(), + [&](const auto& lift) + { + return lift->name() == _lift_destination->lift_name; + }); + + bool inside_lift = false; + if (l_it != graph.known_lifts().end()) + { + inside_lift = (*l_it)->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()); + } + _lift_destination = nullptr; + _initial_time_idle_outside_lift = std::nullopt; + } + else + { + _initial_time_idle_outside_lift = now; + } + } } } 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 56b0127ca..016d83176 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -679,6 +679,8 @@ class RobotContext void _check_lift_state(const rmf_lift_msgs::msg::LiftState& state); std::shared_ptr _lift_destination; rmf_rxcpp::subscription_guard _lift_subscription; + std::optional _initial_time_idle_outside_lift; + }; using RobotContextPtr = std::shared_ptr; From 594d5fafd764cefd6b55b78aab3c218a70724572 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 7 Nov 2023 14:30:10 +0800 Subject: [PATCH 48/89] Charging schedule sorting and mid-performaction cancel fixes Signed-off-by: Xiyu Oh --- rmf_charging_schedule/rmf_charging_schedule/main.py | 2 ++ .../src/rmf_fleet_adapter/events/PerformAction.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/rmf_charging_schedule/rmf_charging_schedule/main.py b/rmf_charging_schedule/rmf_charging_schedule/main.py index cf27f8360..9d90062c7 100644 --- a/rmf_charging_schedule/rmf_charging_schedule/main.py +++ b/rmf_charging_schedule/rmf_charging_schedule/main.py @@ -49,6 +49,8 @@ def __eq__(self, other): 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): 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..f55556f84 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp @@ -224,6 +224,7 @@ void PerformAction::Active::cancel() _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); _finished(); + auto self = shared_from_this(); if (auto data = _execution_data.lock()) data->okay = false; } From bd7feac9d0bbf845523c6d41052b88c2f38b7355 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 10 Nov 2023 08:45:47 +0000 Subject: [PATCH 49/89] Fix cancellation of WaitForCharge Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 5 +++++ .../src/rmf_fleet_adapter/phases/WaitForCharge.cpp | 7 ++++++- 2 files changed, 11 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 960010cee..72ff7f493 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1134,6 +1134,11 @@ void FleetUpdateHandle::Implementation::update_charging_assignments( 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; 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 2af040ed3..cd48937c2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -49,7 +49,7 @@ void WaitForCharge::Active::emergency_alarm(const bool) //============================================================================== void WaitForCharge::Active::cancel() { - // TODO + _status_publisher.get_subscriber().on_completed(); } //============================================================================== @@ -78,6 +78,11 @@ WaitForCharge::Active::Active( _description = "Charging [" + _context->requester_id() + "] to [" + 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; _status_publisher.get_subscriber().on_next(initial_msg); From 40c7a81628d9fb3b56d25449b7d4da1829a8ae0c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 12 Nov 2023 11:13:27 +0000 Subject: [PATCH 50/89] Truncate schedule horizons for mutex group changes Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/LegacyTask.hpp | 2 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 130 ++++++++++++---- .../rmf_fleet_adapter/events/ExecutePlan.hpp | 3 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 15 +- .../events/LockMutexGroup.cpp | 147 +++++++++++++----- .../events/LockMutexGroup.hpp | 21 ++- .../events/WaitForTraffic.cpp | 19 ++- .../events/WaitForTraffic.hpp | 5 +- .../rmf_fleet_adapter/phases/MoveRobot.cpp | 17 +- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 4 +- .../rmf_fleet_adapter/phases/RequestLift.cpp | 19 ++- .../rmf_fleet_adapter/phases/RequestLift.hpp | 4 +- .../test/phases/test_RequestLift.cpp | 2 +- 13 files changed, 300 insertions(+), 88 deletions(-) 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/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 45f0a7860..ec6908441 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -47,7 +47,7 @@ struct LegacyPhaseWrapper std::shared_ptr phase_, rmf_traffic::Time time_, rmf_traffic::Dependencies dependencies_, - std::string mutex_group_dependency_) + std::optional mutex_group_dependency_) : phase(std::move(phase_)), time(time_), dependencies(std::move(dependencies_)), @@ -59,7 +59,7 @@ struct LegacyPhaseWrapper std::shared_ptr phase; rmf_traffic::Time time; rmf_traffic::Dependencies dependencies; - std::string mutex_group_dependency; + std::optional mutex_group_dependency; }; using LegacyPhases = std::vector; @@ -74,7 +74,7 @@ using Move = phases::MoveRobot::PendingPhase; //============================================================================== 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) @@ -97,7 +97,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor agv::RobotContextPtr context, LegacyPhases& phases, const rmf_traffic::agv::Plan::Waypoint& waypoint_, - rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, bool& continuous) : waypoint(waypoint_), _context(std::move(context)), @@ -117,7 +117,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _phases.emplace_back( std::make_shared( _context, dock.dock_name()), - _event_start_time, waypoint.dependencies(), ""); + _event_start_time, waypoint.dependencies(), std::nullopt); _continuous = false; } @@ -131,7 +131,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.name(), _context->requester_id(), _event_start_time + open.duration()), - _event_start_time, waypoint.dependencies(), ""); + _event_start_time, waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -146,7 +146,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, waypoint.dependencies(), ""); + _event_start_time, waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -162,7 +162,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _event_start_time, phases::RequestLift::Located::Outside, _plan_id), - _event_start_time, waypoint.dependencies(), ""); + _event_start_time, waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -197,7 +197,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor phases::RequestLift::Located::Inside, _plan_id, localize), - _event_start_time, waypoint.dependencies(), ""); + _event_start_time, waypoint.dependencies(), std::nullopt); _moving_lift = false; _continuous = true; @@ -212,7 +212,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, waypoint.dependencies(), ""); + _event_start_time, waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -231,7 +231,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor agv::RobotContextPtr _context; LegacyPhases& _phases; rmf_traffic::Time _event_start_time; - rmf_traffic::PlanId _plan_id; + PlanIdPtr _plan_id; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); @@ -250,7 +250,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( @@ -341,7 +341,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) { @@ -460,7 +460,7 @@ class Printer : public rmf_traffic::agv::Graph::Lane::Executor //============================================================================== 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::schedule::Itinerary full_itinerary, const rmf_task::Event::AssignIDPtr& event_id, @@ -484,6 +484,11 @@ std::optional ExecutePlan::make( // } // } + auto plan_id = std::make_shared(recommended_plan_id); + auto initial_itinerary = + std::make_shared(full_itinerary); + auto previous_itinerary = initial_itinerary; + const auto& graph = context->navigation_graph(); std::optional finish_time_estimate; @@ -505,7 +510,7 @@ std::optional ExecutePlan::make( plan.get_waypoints(); std::vector move_through; - std::string current_mutex_group; + std::optional current_mutex_group; LegacyPhases legacy_phases; while (!waypoints.empty()) @@ -536,13 +541,65 @@ std::optional ExecutePlan::make( } } - std::string mutex_group_dependency; - if (new_mutex_group != current_mutex_group) + const bool mutex_group_change = + (!new_mutex_group.empty() && current_mutex_group.has_value()) + || ( + current_mutex_group.has_value() + && current_mutex_group->mutex_group != new_mutex_group + ); + + if (mutex_group_change) { if (!new_mutex_group.empty()) { if (move_through.size() > 1) { + const auto& last_wp = move_through.back(); + + const rmf_traffic::Time hold_time = last_wp.time(); + const Eigen::Vector3d hold_position = last_wp.position(); + std::string hold_map; + if (last_wp.graph_index().has_value()) + { + hold_map = + graph.get_waypoint(*last_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 [%s] transition needed " + "by robot [%s]. There are [%lu] remaining waypoints. Please " + "report this situation to the maintainers of RMF.", + new_mutex_group.c_str(), + context->requester_id().c_str(), + waypoints.size()); + } + } + + std::size_t excluded_route = 0; + for (const auto& c : last_wp.arrival_checkpoints()) + { + excluded_route = std::max(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()); + } + previous_itinerary->erase(previous_itinerary->begin()+excluded_route); + legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), @@ -551,7 +608,19 @@ std::optional ExecutePlan::make( move_through.clear(); waypoints.erase(waypoints.begin(), it); - current_mutex_group = std::move(new_mutex_group); + + auto next_itinerary = std::make_shared< + rmf_traffic::schedule::Itinerary>(full_itinerary); + current_mutex_group = LockMutexGroup::Data{ + new_mutex_group, + hold_map, + hold_position, + hold_time, + plan_id, + next_itinerary + }; + + previous_itinerary = next_itinerary; // We treat this the same as an event occurring to indicate that // we should keep looping. @@ -559,8 +628,10 @@ std::optional ExecutePlan::make( break; } } - - current_mutex_group = std::move(new_mutex_group); + else + { + current_mutex_group = std::nullopt; + } } move_through.push_back(*it); @@ -572,7 +643,7 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies(), mutex_group_dependency); + it->time(), it->dependencies(), current_mutex_group); } move_through.clear(); @@ -689,13 +760,13 @@ std::optional ExecutePlan::make( } else { - if (!head->mutex_group_dependency.empty()) + if (head->mutex_group_dependency.has_value()) { standbys.push_back( [ context, event_id, - mutex_group = head->mutex_group_dependency + mutex_group = head->mutex_group_dependency.value() ](UpdateFn update) { return LockMutexGroup::Standby::make( @@ -743,7 +814,7 @@ std::optional ExecutePlan::make( standbys, state, std::move(update))->begin([]() {}, std::move(finished)); std::size_t attempts = 0; - while (!context->itinerary().set(plan_id, std::move(full_itinerary))) + 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 @@ -756,17 +827,18 @@ std::optional ExecutePlan::make( 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, + *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."); + "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(); + *plan_id = context->itinerary().assign_plan_id(); if (++attempts > 5) { 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..f19a0ee11 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 @@ -41,7 +42,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 cdfb4af34..6193b3753 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -266,8 +266,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()); + } } const auto& estimate = diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index 869f71aae..f691b4492 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -24,17 +24,17 @@ namespace events { auto LockMutexGroup::Standby::make( agv::RobotContextPtr context, const AssignIDPtr& id, - std::string mutex_group) + Data data) -> std::shared_ptr { auto standby = std::shared_ptr(new Standby); standby->_context = std::move(context); standby->_state = rmf_task::events::SimpleEventState::make( id->assign(), - "Lock mutex group [" + mutex_group + "]", + "Lock mutex group [" + data.mutex_group + "]", "Waiting for the mutex group to be locked", rmf_task::Event::Status::Standby, {}, standby->_context->clock()); - standby->_mutex_group = std::move(mutex_group); + standby->_data = std::move(data); return standby; } @@ -55,7 +55,7 @@ auto LockMutexGroup::Standby::begin( std::function, std::function finished) -> ActivePtr { - return Active::make(_context, _state, std::move(finished), _mutex_group); + return Active::make(_context, _state, std::move(finished), _data); } //============================================================================== @@ -63,47 +63,15 @@ auto LockMutexGroup::Active::make( agv::RobotContextPtr context, rmf_task::events::SimpleEventStatePtr state, std::function finished, - std::string mutex_group) -> std::shared_ptr + Data data) -> std::shared_ptr { auto active = std::shared_ptr(new Active); active->_context = std::move(context); active->_state = std::move(state); active->_finished = std::move(finished); - active->_mutex_group = std::move(mutex_group); + active->_data = std::move(data); active->_state->update_status(Status::Underway); - - using MutexGroupStatesPtr = - std::shared_ptr; - - active->_listener = active->_context->node()->mutex_group_states() - .observe_on(rxcpp::identity_same_worker(active->_context->worker())) - .subscribe( - [weak = active->weak_from_this()]( - const MutexGroupStatesPtr& mutex_group_states) - { - const auto self = weak.lock(); - if (!self) - return; - - for (const auto& assignment : mutex_group_states->assignments) - { - if (assignment.group == self->_mutex_group) - { - if (assignment.claimed == self->_context->requester_id()) - { - const auto finished = self->_finished; - self->_finished = nullptr; - if (finished) - { - finished(); - return; - } - } - } - } - }); - - active->_context->set_mutex_group(active->_mutex_group); + active->_initialize(); return active; } @@ -153,5 +121,106 @@ void LockMutexGroup::Active::kill() cancel(); } +//============================================================================== +void LockMutexGroup::Active::_initialize() +{ + using MutexGroupStatesPtr = + std::shared_ptr; + + const auto t_buffer = std::chrono::seconds(10); + const auto zero = Eigen::Vector3d::Zero(); + rmf_traffic::Trajectory hold_traj; + hold_traj.insert(_data.hold_time, _data.hold_position, zero); + hold_traj.insert(_data.hold_time + t_buffer, _data.hold_position, zero); + rmf_traffic::Route hold(_data.hold_map, std::move(hold_traj)); + _schedule({hold}); + + const auto cumulative_delay = _context->now() - _data.hold_time; + _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); + + _listener = _context->node()->mutex_group_states() + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [weak = weak_from_this()]( + const MutexGroupStatesPtr& mutex_group_states) + { + const auto self = weak.lock(); + if (!self) + return; + + for (const auto& assignment : mutex_group_states->assignments) + { + if (assignment.group == self->_data.mutex_group) + { + if (assignment.claimed == self->_context->requester_id()) + { + const auto finished = self->_finished; + self->_finished = nullptr; + if (finished) + { + self->_schedule(*self->_data.resume_itinerary); + finished(); + return; + } + } + } + } + }); + + _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; + + const auto cumulative_delay = + self->_context->now() - self->_data.hold_time; + self->_context->itinerary().cumulative_delay(plan_id, cumulative_delay); + }); + + _context->set_mutex_group(_data.mutex_group); +} + +//============================================================================== +void LockMutexGroup::Active::_schedule( + rmf_traffic::schedule::Itinerary itinerary) const +{ + bool scheduled = false; + std::size_t attempts = 0; + while (!scheduled) + { + if (++attempts > 5) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Repeatedly failled attempts to update schedule during LockMutexGroup " + "action for robot [%s]. Last attempted value was [%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.", + _context->requester_id().c_str(), + *_data.plan_id); + break; + } + + *_data.plan_id = _context->itinerary().assign_plan_id(); + scheduled = _context->itinerary().set(*_data.plan_id, itinerary); + + if (!scheduled) + { + RCLCPP_ERROR( + _context->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.", + *_data.plan_id, + _context->itinerary().current_plan_id(), + _context->requester_id().c_str()); + } + } +} + } // 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 index f82ec16e2..eee2acec3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -30,13 +30,23 @@ namespace events { class LockMutexGroup : public rmf_task_sequence::Event { public: + struct Data + { + std::string mutex_group; + std::string hold_map; + Eigen::Vector3d hold_position; + rmf_traffic::Time hold_time; + std::shared_ptr plan_id; + std::shared_ptr resume_itinerary; + }; + class Standby : public rmf_task_sequence::Event::Standby { public: static std::shared_ptr make( agv::RobotContextPtr context, const AssignIDPtr& id, - std::string mutex_group); + Data data); ConstStatePtr state() const final; @@ -50,7 +60,7 @@ class LockMutexGroup : public rmf_task_sequence::Event Standby() = default; agv::RobotContextPtr _context; rmf_task::events::SimpleEventStatePtr _state; - std::string _mutex_group; + Data _data; }; class Active @@ -62,7 +72,7 @@ class LockMutexGroup : public rmf_task_sequence::Event agv::RobotContextPtr context, rmf_task::events::SimpleEventStatePtr state, std::function finished, - std::string mutex_group); + Data data); ConstStatePtr state() const final; @@ -77,11 +87,14 @@ class LockMutexGroup : public rmf_task_sequence::Event void kill() final; private: + void _initialize(); + void _schedule(rmf_traffic::schedule::Itinerary itinerary) const; agv::RobotContextPtr _context; rmf_task::events::SimpleEventStatePtr _state; std::function _finished; - std::string _mutex_group; rmf_rxcpp::subscription_guard _listener; + rclcpp::TimerBase::SharedPtr _delay_timer; + Data _data; }; }; 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..928a52da7 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,24 @@ auto WaitForTraffic::Standby::begin( std::function, std::function finished) -> ActivePtr { + 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, 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..43f432fba 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; 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..c8d7c032d 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)}, @@ -100,8 +100,21 @@ MoveRobot::PendingPhase::PendingPhase( //============================================================================== 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); } //============================================================================== 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 3d778f2ef..41b365e01 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; }; 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 1620a6540..14b67e7bd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -373,7 +373,7 @@ RequestLift::PendingPhase::PendingPhase( std::string destination, rmf_traffic::Time expected_finish, Located located, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, std::optional localize) : _context(std::move(context)), _lift_name(std::move(lift_name)), @@ -392,13 +392,28 @@ RequestLift::PendingPhase::PendingPhase( //============================================================================== std::shared_ptr RequestLift::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 RequestLift action for robot [%s]. This is " + "a critical internal error, please report this bug to the RMF " + "maintainers.", + _context->requester_id().c_str()); + } + return ActivePhase::make( _context, _lift_name, _destination, _expected_finish, _located, - _plan_id, + plan_id, _localize_after); } 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 a117a0e33..d05249aef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -114,7 +114,7 @@ struct RequestLift std::string destination, rmf_traffic::Time expected_finish, Located located, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, std::optional localize = std::nullopt); std::shared_ptr begin() override; @@ -134,7 +134,7 @@ struct RequestLift std::string _destination; rmf_traffic::Time _expected_finish; Located _located; - rmf_traffic::PlanId _plan_id; + PlanIdPtr _plan_id; std::optional _localize_after; std::string _description; }; diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index 1d58a907e..a3fc043c7 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -72,7 +72,7 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") destination, context->now() + std::chrono::seconds(5), RequestLift::Located::Outside, - 0 + std::make_shared(0) ); auto active_phase = pending_phase->begin(); From 50c11c6a84e3473b9d1c4b420f2a0d4e4b5afe84 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Tue, 14 Nov 2023 15:35:09 +0800 Subject: [PATCH 51/89] Fix shared pointer from prev fix Signed-off-by: Xiyu Oh --- .../src/rmf_fleet_adapter/events/PerformAction.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f55556f84..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,8 +223,8 @@ void PerformAction::Active::cancel() { _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); - _finished(); auto self = shared_from_this(); + _finished(); if (auto data = _execution_data.lock()) data->okay = false; } From 4956443245f1d6ef8ec507a902871148541a6039 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 15 Nov 2023 08:56:05 +0000 Subject: [PATCH 52/89] Ironing out mutex group behaviors -- need atomic switching Signed-off-by: Michael X. Grey --- .../src/mutex_group_supervisor/main.cpp | 6 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 101 +++++++------- .../rmf_fleet_adapter/agv/RobotContext.cpp | 13 ++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 + .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 31 +++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 131 +++++++++++++----- .../events/LockMutexGroup.cpp | 39 +++++- .../events/WaitForTraffic.cpp | 17 +++ .../rmf_fleet_adapter/phases/MoveRobot.hpp | 19 ++- 9 files changed, 274 insertions(+), 87 deletions(-) diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index 1dfe179cc..b76a4369a 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -39,7 +39,7 @@ using MutexGroupRequest = rmf_fleet_msgs::msg::MutexGroupRequest; using MutexGroupStates = rmf_fleet_msgs::msg::MutexGroupStates; using MutextGroupAssignment = rmf_fleet_msgs::msg::MutexGroupAssignment; -class Node : rclcpp::Node +class Node : public rclcpp::Node { public: Node() @@ -199,5 +199,7 @@ class Node : rclcpp::Node 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/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index 1d9677600..6dc737dae 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1051,54 +1051,59 @@ void EasyCommandHandle::follow_new_path( std::vector queue; const auto& current_location = context->location(); - // std::cout << "Locations: " << current_location.size() << std::endl; - // for (const auto& l : current_location) - // { - // std::cout << " -- "; - // if (l.lane().has_value()) - // { - // std::cout << "lane[" << *l.lane() << "] "; - // Printer printer; - // const auto& lane = graph.get_lane(*l.lane()); - // if (lane.entry().event()) - // { - // std::cout << " [entry "; - // lane.entry().event()->execute(printer); - // std::cout << "] "; - // } - // const auto& i_wp0 = lane.entry().waypoint_index(); - // const auto& wp0 = graph.get_waypoint(i_wp0); - // const auto& i_wp1 = lane.exit().waypoint_index(); - // const auto& wp1 = graph.get_waypoint(i_wp1); - // std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() - // << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - // if (lane.exit().event()) - // { - // std::cout << "[exit "; - // lane.exit().event()->execute(printer); - // std::cout << "]"; - // } - // std::cout << " | "; - // } - // std::cout << l.waypoint(); - // if (l.location().has_value()) - // { - // std::cout << " | " << l.location()->transpose(); - // } - // std::cout << std::endl; - // } - // std::cout << "Num waypoints: " << waypoints.size() << std::endl; - // for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) - // { - // std::cout << " --"; - // if (wp.graph_index().has_value()) - // { - // std::cout << " index " << *wp.graph_index(); - // const auto& gwp = graph.get_waypoint(*wp.graph_index()); - // std::cout << " [" << gwp.get_map_name() << "]"; - // } - // std::cout << " " << wp.position().transpose() << std::endl; - // } + std::cout << context->requester_id() << " Locations: " << current_location.size() << std::endl; + for (const auto& l : current_location) + { + std::cout << " -- "; + if (l.lane().has_value()) + { + std::cout << "lane[" << *l.lane() << "] "; + Printer printer; + const auto& lane = graph.get_lane(*l.lane()); + if (lane.entry().event()) + { + std::cout << " [entry "; + lane.entry().event()->execute(printer); + std::cout << "] "; + } + const auto& i_wp0 = lane.entry().waypoint_index(); + const auto& wp0 = graph.get_waypoint(i_wp0); + const auto& i_wp1 = lane.exit().waypoint_index(); + const auto& wp1 = graph.get_waypoint(i_wp1); + std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() + << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + if (lane.exit().event()) + { + std::cout << "[exit "; + lane.exit().event()->execute(printer); + std::cout << "]"; + } + std::cout << " | "; + } + std::cout << l.waypoint(); + if (l.location().has_value()) + { + std::cout << " | " << l.location()->transpose(); + } + std::cout << std::endl; + } + std::cout << "Num waypoints: " << waypoints.size() << std::endl; + for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) + { + std::cout << " --"; + if (wp.graph_index().has_value()) + { + std::cout << " index " << *wp.graph_index(); + const auto& gwp = graph.get_waypoint(*wp.graph_index()); + if (!gwp.in_mutex_group().empty()) + { + std::cout << " *" << gwp.in_mutex_group(); + } + + std::cout << " [" << gwp.get_map_name() << "]"; + } + std::cout << " " << wp.position().transpose() << std::endl; + } bool found_connection = false; std::size_t i0 = 0; 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 b2a8e8bef..6e196d0aa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1031,6 +1031,12 @@ const std::string& RobotContext::current_mutex_group() const return _mutex_group; } +//============================================================================== +bool RobotContext::obtained_mutex_group() const +{ + return _obtained_mutex_group; +} + //============================================================================== void RobotContext::set_mutex_group(std::string group) { @@ -1045,6 +1051,7 @@ void RobotContext::set_mutex_group(std::string group) return; } + _obtained_mutex_group = false; _mutex_group_claim_time = _node->now(); _mutex_group = std::move(group); _publish_mutex_group_request(); @@ -1053,6 +1060,7 @@ void RobotContext::set_mutex_group(std::string group) //============================================================================== void RobotContext::release_mutex_group() { + _obtained_mutex_group = false; if (_mutex_group.empty()) return; @@ -1062,6 +1070,7 @@ void RobotContext::release_mutex_group() .claimer(requester_id()) .claim_time(_mutex_group_claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + _mutex_group = ""; } //============================================================================== @@ -1249,8 +1258,12 @@ void RobotContext::_check_mutex_groups( return; if (assignment.group == _mutex_group) + { + _obtained_mutex_group = true; return; + } + _obtained_mutex_group = false; _node->mutex_group_request()->publish( rmf_fleet_msgs::build() .group(assignment.group) 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 8373dfee2..dd420e009 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -563,6 +563,9 @@ class RobotContext /// What mutex group is currently being locked. const std::string& current_mutex_group() const; + /// Have we obtained the current mutex group yet? + bool obtained_mutex_group() const; + /// Set the mutex group that this robot needs to have locked. void set_mutex_group(std::string group); @@ -715,6 +718,7 @@ class RobotContext void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); void _publish_mutex_group_request(); std::string _mutex_group; + bool _obtained_mutex_group = false; builtin_interfaces::msg::Time _mutex_group_claim_time; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; 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 7c9e07c5c..37ca222b1 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 @@ -165,6 +165,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) { @@ -360,6 +366,12 @@ 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; } @@ -427,6 +439,25 @@ rmf_traffic::agv::Graph parse_graph( } } + std::cout << " ========= Mutex groups:" << std::endl; + for (std::size_t i=0; i < graph.num_waypoints(); ++i) + { + const auto& m = graph.get_waypoint(i).in_mutex_group(); + if (!m.empty()) + { + std::cout << " -- " << i << ": " << m << std::endl; + } + } + + for (std::size_t i=0; i < graph.num_lanes(); ++i) + { + const auto& m = graph.get_lane(i).properties().in_mutex_group(); + if (!m.empty()) + { + std::cout << " -- " << i << ": " << m << std::endl; + } + } + return graph; } 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 ec6908441..d9fbaff27 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -71,6 +71,18 @@ 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, @@ -286,6 +298,12 @@ std::optional search_for_door_group( ++tail; for (auto it = head; it != tail; ++it) { + if (it->mutex_group_dependency.has_value()) + { + door_group.push_back(make_wait_for_mutex( + context, id, it->mutex_group_dependency.value())); + } + door_group.push_back( [legacy = it->phase, context, id](UpdateFn update) { @@ -395,6 +413,12 @@ std::optional search_for_lift_group( ++tail; for (auto it = head; it != tail; ++it) { + if (it->mutex_group_dependency.has_value()) + { + lift_group.push_back(make_wait_for_mutex( + context, event_id, it->mutex_group_dependency.value())); + } + lift_group.push_back( [legacy = it->phase, context, event_id](UpdateFn update) { @@ -526,6 +550,7 @@ std::optional ExecutePlan::make( graph.get_waypoint(*it->graph_index()).in_mutex_group(); if (!group.empty()) { + std::cout << " === " << __LINE__ << ": " << group << std::endl; new_mutex_group = group; } } @@ -536,33 +561,38 @@ std::optional ExecutePlan::make( const auto& group = lane.properties().in_mutex_group(); if (!group.empty()) { + std::cout << " === " << __LINE__ << ": " << group << std::endl; new_mutex_group = group; break; } } const bool mutex_group_change = - (!new_mutex_group.empty() && current_mutex_group.has_value()) + (!new_mutex_group.empty() && !current_mutex_group.has_value()) || ( current_mutex_group.has_value() && current_mutex_group->mutex_group != new_mutex_group ); + std::cout << new_mutex_group << " | " << current_mutex_group.has_value(); + if (current_mutex_group.has_value()) + std::cout << " | " << current_mutex_group->mutex_group; + std::cout << std::endl; + if (mutex_group_change) { + std::cout << " === " << __LINE__ << ": " << new_mutex_group << std::endl; if (!new_mutex_group.empty()) { - if (move_through.size() > 1) + const auto make_current_mutex_group = [&](const rmf_traffic::agv::Plan::Waypoint& wp) { - const auto& last_wp = move_through.back(); - - const rmf_traffic::Time hold_time = last_wp.time(); - const Eigen::Vector3d hold_position = last_wp.position(); + const rmf_traffic::Time hold_time = wp.time(); + const Eigen::Vector3d hold_position = wp.position(); std::string hold_map; - if (last_wp.graph_index().has_value()) + if (wp.graph_index().has_value()) { hold_map = - graph.get_waypoint(*last_wp.graph_index()).get_map_name(); + graph.get_waypoint(*wp.graph_index()).get_map_name(); } else { @@ -590,8 +620,23 @@ std::optional ExecutePlan::make( } } + auto next_itinerary = std::make_shared< + rmf_traffic::schedule::Itinerary>(full_itinerary); + return LockMutexGroup::Data{ + new_mutex_group, + hold_map, + hold_position, + hold_time, + plan_id, + next_itinerary + }; + }; + + if (move_through.size() > 1) + { + auto next_mutex_group = make_current_mutex_group(move_through.back()); std::size_t excluded_route = 0; - for (const auto& c : last_wp.arrival_checkpoints()) + for (const auto& c : move_through.back().arrival_checkpoints()) { excluded_route = std::max(excluded_route, c.route_id+1); auto& r = previous_itinerary->at(c.route_id); @@ -600,6 +645,7 @@ std::optional ExecutePlan::make( } previous_itinerary->erase(previous_itinerary->begin()+excluded_route); + std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), @@ -609,24 +655,34 @@ std::optional ExecutePlan::make( move_through.clear(); waypoints.erase(waypoints.begin(), it); - auto next_itinerary = std::make_shared< - rmf_traffic::schedule::Itinerary>(full_itinerary); - current_mutex_group = LockMutexGroup::Data{ - new_mutex_group, - hold_map, - hold_position, - hold_time, - plan_id, - next_itinerary - }; + current_mutex_group = next_mutex_group; - previous_itinerary = next_itinerary; + previous_itinerary = current_mutex_group->resume_itinerary; // 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_group from this point. + if (move_through.empty()) + { + current_mutex_group = make_current_mutex_group(*it); + previous_itinerary = current_mutex_group->resume_itinerary; + std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; + } + else + { + assert(move_through.size() == 1); + current_mutex_group = make_current_mutex_group( + move_through.back()); + previous_itinerary = current_mutex_group->resume_itinerary; + std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; + } + } } else { @@ -744,6 +800,8 @@ std::optional ExecutePlan::make( std::vector standbys; auto head = legacy_phases.cbegin(); const auto end = legacy_phases.cend(); + std::stringstream ss; + ss << " ===== Execution plan for " << context->requester_id() << ":"; while (head != end) { if (const auto door = @@ -762,20 +820,14 @@ std::optional ExecutePlan::make( { if (head->mutex_group_dependency.has_value()) { - standbys.push_back( - [ - context, - event_id, - mutex_group = head->mutex_group_dependency.value() - ](UpdateFn update) - { - return LockMutexGroup::Standby::make( - context, event_id, mutex_group); - }); + ss << "\n -- Adding mutex group [" << head->mutex_group_dependency->mutex_group << "]"; + standbys.push_back(make_wait_for_mutex( + context, event_id, head->mutex_group_dependency.value())); } if (head->phase) { + ss << "\n -- " << head->phase->description(); standbys.push_back( [legacy = head->phase, context, event_id](UpdateFn update) { @@ -786,12 +838,20 @@ std::optional ExecutePlan::make( if (!head->dependencies.empty()) { + ss << "\n -- wait for:"; + for (const auto& d : head->dependencies) + { + ss << " " << d.on_participant; + } + standbys.push_back(make_wait_for_traffic( context, plan_id, head->dependencies, head->time, event_id)); } ++head; } + + std::cout << ss.str() << std::endl; } if (tail_period.has_value() && !legacy_phases.empty()) @@ -809,10 +869,6 @@ 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, *initial_itinerary)) { @@ -826,7 +882,8 @@ std::optional ExecutePlan::make( 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.", + "[%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(), @@ -853,6 +910,10 @@ 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)); + return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index f691b4492..e8606498f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -70,7 +70,6 @@ auto LockMutexGroup::Active::make( active->_state = std::move(state); active->_finished = std::move(finished); active->_data = std::move(data); - active->_state->update_status(Status::Underway); active->_initialize(); return active; @@ -124,9 +123,38 @@ void LockMutexGroup::Active::kill() //============================================================================== void LockMutexGroup::Active::_initialize() { + _state->update_status(State::Status::Underway); using MutexGroupStatesPtr = std::shared_ptr; + if (_context->current_mutex_group() == _data.mutex_group) + { + if (_context->obtained_mutex_group()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Mutex group [%s] was already locked for [%s]", + _data.mutex_group.c_str(), + _context->requester_id().c_str()); + // We don't need to do anything further, we already got the mutex group + // previously. + _context->worker().schedule( + [finished = _finished](const auto&) + { + finished(); + }); + return; + } + } + + _state->update_log().info( + "Waiting to lock mutex group [" + _data.mutex_group + "]"); + RCLCPP_INFO( + _context->node()->get_logger(), + "Waiting to lock mutex group [%s] for robot [%s]", + _data.mutex_group.c_str(), + _context->requester_id().c_str()); + const auto t_buffer = std::chrono::seconds(10); const auto zero = Eigen::Vector3d::Zero(); rmf_traffic::Trajectory hold_traj; @@ -158,7 +186,14 @@ void LockMutexGroup::Active::_initialize() self->_finished = nullptr; if (finished) { + std::cout << " === FINISHED " << __LINE__ << std::endl; self->_schedule(*self->_data.resume_itinerary); + self->_state->update_status(State::Status::Completed); + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Finished locking mutex group [%s] for robot [%s]", + self->_data.mutex_group.c_str(), + self->_context->requester_id().c_str()); finished(); return; } @@ -180,6 +215,8 @@ void LockMutexGroup::Active::_initialize() self->_context->itinerary().cumulative_delay(plan_id, cumulative_delay); }); + std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() + << " TO " << _data.mutex_group << std::endl; _context->set_mutex_group(_data.mutex_group); } 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 928a52da7..7b1a84aed 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -59,6 +59,10 @@ 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) { @@ -227,6 +231,11 @@ void WaitForTraffic::Active::_consider_going() bool all_dependencies_reached = true; for (const auto& dep : _dependencies) { + const auto current = _context->schedule()->get_current_plan_id(dep.dependency().on_participant); + std::cout << " -- " << dep.dependency().on_participant << ":" << dep.dependency().on_plan + << " vs current " << current.value_or((std::size_t)(-1)) + << " | " << dep.reached() << " | " << dep.deprecated() + << std::endl; if (!dep.reached() && !dep.deprecated()) all_dependencies_reached = false; } @@ -236,6 +245,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(); } @@ -248,6 +261,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/phases/MoveRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp index 41b365e01..5a02ecf94 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -284,10 +284,19 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (!context->current_mutex_group().empty()) { - const auto adjusted_now = now + new_cumulative_delay; + const auto adjusted_now = now - new_cumulative_delay; const auto& graph = context->navigation_graph(); + std::size_t count = 0; for (const auto& wp : self->_waypoints) { + if (count == 0) + { + // The first waypoint doesn't always have a mutex group + // associated. + ++count; + continue; + } + if (wp.time() > adjusted_now) { break; @@ -299,10 +308,17 @@ void MoveRobot::Action::operator()(const Subscriber& s) .in_mutex_group(); if (g.empty()) { + std::cout << __LINE__ << ": Releasing mutex for " << *wp.graph_index() + << " index " << count + << " | " << rmf_traffic::time::to_seconds(wp.time().time_since_epoch()) + << " vs " << rmf_traffic::time::to_seconds(adjusted_now.time_since_epoch()) + << std::endl; context->release_mutex_group(); break; } } + + ++count; } } }); @@ -325,6 +341,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) const auto& graph = self->_context->navigation_graph(); if (graph.get_waypoint(*last_index).in_mutex_group().empty()) { + std::cout << __LINE__ << ": Releasing mutex at end of path" << std::endl; self->_context->release_mutex_group(); } } From 04f13784ffd7bb6da9bb3f9a55156e467c8377d7 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 15 Nov 2023 15:21:17 +0000 Subject: [PATCH 53/89] Added atomic switching -- need to fix movement continuity bug Signed-off-by: Michael X. Grey --- .../src/mutex_group_supervisor/main.cpp | 6 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 88 +++++++----- .../rmf_fleet_adapter/agv/RobotContext.cpp | 136 +++++++++++------- .../rmf_fleet_adapter/agv/RobotContext.hpp | 20 +-- .../events/LockMutexGroup.cpp | 34 +++-- .../events/LockMutexGroup.hpp | 1 + .../rmf_fleet_adapter/phases/MoveRobot.hpp | 2 +- 7 files changed, 175 insertions(+), 112 deletions(-) diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index b76a4369a..9967b387d 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -167,8 +167,10 @@ class Node : public rclcpp::Node } std::string claimer; + builtin_interfaces::msg::Time claim_time; if (earliest.has_value()) { + claim_time = earliest->first; claimer = earliest->second; } bool group_found = false; @@ -177,6 +179,7 @@ class Node : public rclcpp::Node if (a.group == group) { a.claimed = claimer; + a.claim_time = claim_time; group_found = true; break; } @@ -186,7 +189,8 @@ class Node : public rclcpp::Node latest_states.assignments.push_back( rmf_fleet_msgs::build() .group(group) - .claimed(claimer)); + .claimed(claimer) + .claim_time(claim_time)); } } 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 6dc737dae..8141d3151 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1051,41 +1051,46 @@ void EasyCommandHandle::follow_new_path( std::vector queue; const auto& current_location = context->location(); - std::cout << context->requester_id() << " Locations: " << current_location.size() << std::endl; - for (const auto& l : current_location) - { - std::cout << " -- "; - if (l.lane().has_value()) + auto print_location = [&](const rmf_traffic::agv::Plan::Start& l) { - std::cout << "lane[" << *l.lane() << "] "; - Printer printer; - const auto& lane = graph.get_lane(*l.lane()); - if (lane.entry().event()) + std::stringstream ss; + if (l.lane().has_value()) { - std::cout << " [entry "; - lane.entry().event()->execute(printer); - std::cout << "] "; + ss << "lane[" << *l.lane() << "] "; + Printer printer; + const auto& lane = graph.get_lane(*l.lane()); + if (lane.entry().event()) + { + ss << " [entry "; + lane.entry().event()->execute(printer); + ss << "] "; + } + const auto& i_wp0 = lane.entry().waypoint_index(); + const auto& wp0 = graph.get_waypoint(i_wp0); + const auto& i_wp1 = lane.exit().waypoint_index(); + const auto& wp1 = graph.get_waypoint(i_wp1); + ss << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() + << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + if (lane.exit().event()) + { + ss << "[exit "; + lane.exit().event()->execute(printer); + ss << "]"; + } + ss << " | "; } - const auto& i_wp0 = lane.entry().waypoint_index(); - const auto& wp0 = graph.get_waypoint(i_wp0); - const auto& i_wp1 = lane.exit().waypoint_index(); - const auto& wp1 = graph.get_waypoint(i_wp1); - std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() - << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - if (lane.exit().event()) + ss << l.waypoint(); + if (l.location().has_value()) { - std::cout << "[exit "; - lane.exit().event()->execute(printer); - std::cout << "]"; + ss << " | " << l.location()->transpose(); } - std::cout << " | "; - } - std::cout << l.waypoint(); - if (l.location().has_value()) - { - std::cout << " | " << l.location()->transpose(); - } - std::cout << std::endl; + return ss.str(); + }; + + std::cout << context->requester_id() << " Locations: " << current_location.size() << std::endl; + for (const auto& l : current_location) + { + std::cout << " -- xyz " << print_location(l) << std::endl; } std::cout << "Num waypoints: " << waypoints.size() << std::endl; for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) @@ -1107,13 +1112,17 @@ void EasyCommandHandle::follow_new_path( bool found_connection = false; std::size_t i0 = 0; + std::stringstream ss; + ss << "Finding connection for " << context->requester_id(); for (std::size_t i = 0; i < waypoints.size(); ++i) { const auto& wp = waypoints[i]; if (wp.graph_index().has_value()) { + ss << " \n -- " << wp.graph_index().value() << "\n"; for (const auto& l : current_location) { + ss << "location: " << l.waypoint() << "\n"; if (nav_params->in_same_stack(*wp.graph_index(), l.waypoint()) && !l.lane().has_value()) { if (l.location().has_value()) @@ -1127,7 +1136,7 @@ void EasyCommandHandle::follow_new_path( { const double dist = (*l.location() - wp.position().block<2, 1>(0, 0)).norm(); - // std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; + ss << dist << " vs " << nav_params->max_merge_lane_distance << "\n"; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1141,23 +1150,28 @@ void EasyCommandHandle::follow_new_path( i0 = i; } } + else + { + ss << "not the same stack: " << l.waypoint() << " vs " << wp.graph_index().value() << "\n"; + } } } else { + ss << "\n -- " << wp.position().transpose() << "\n"; 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(); - // std::cout << dist << " vs " << nav_params->max_merge_lane_distance << std::endl; + const double dist = (wp.position().block<2, 1>(0, 0) - p_l).norm(); + ss << dist << " vs " << nav_params->max_merge_lane_distance << "\n"; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1185,6 +1199,8 @@ void EasyCommandHandle::follow_new_path( } } + std::cout << ss.str() << std::endl; + if (!found_connection) { // The robot has drifted away from the starting point since the plan started 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 6e196d0aa..56dfd04a0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1026,19 +1026,13 @@ void RobotContext::release_lift() } //============================================================================== -const std::string& RobotContext::current_mutex_group() const +const std::string& RobotContext::locked_mutex_group() const { - return _mutex_group; + return _locked_mutex_group.name; } //============================================================================== -bool RobotContext::obtained_mutex_group() const -{ - return _obtained_mutex_group; -} - -//============================================================================== -void RobotContext::set_mutex_group(std::string group) +void RobotContext::request_mutex_group(std::string group) { if (group.empty()) { @@ -1046,31 +1040,20 @@ void RobotContext::set_mutex_group(std::string group) return; } - if (group == _mutex_group) + if (group == _requesting_mutex_group.name) { return; } - _obtained_mutex_group = false; - _mutex_group_claim_time = _node->now(); - _mutex_group = std::move(group); + _requesting_mutex_group = MutexGroupData{std::move(group), _node->now()}; _publish_mutex_group_request(); } //============================================================================== void RobotContext::release_mutex_group() { - _obtained_mutex_group = false; - if (_mutex_group.empty()) - return; - - _node->mutex_group_request()->publish( - rmf_fleet_msgs::build() - .group(_mutex_group) - .claimer(requester_id()) - .claim_time(_mutex_group_claim_time) - .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); - _mutex_group = ""; + _release_mutex_group(_requesting_mutex_group); + _release_mutex_group(_locked_mutex_group); } //============================================================================== @@ -1257,22 +1240,54 @@ void RobotContext::_check_mutex_groups( if (assignment.claimed != requester_id()) return; - if (assignment.group == _mutex_group) + if (assignment.group == _requesting_mutex_group.name) { - _obtained_mutex_group = true; + if (_locked_mutex_group.name != _requesting_mutex_group.name) + { + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(_locked_mutex_group.name) + .claimer(requester_id()) + .claim_time(_locked_mutex_group.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + + _locked_mutex_group = _requesting_mutex_group; + } + return; } - _obtained_mutex_group = false; - _node->mutex_group_request()->publish( - rmf_fleet_msgs::build() - .group(assignment.group) - .claimer(requester_id()) - .claim_time(_mutex_group_claim_time) - .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + if (assignment.group != _locked_mutex_group.name) + { + // This assignment does not match either the requested nor the currently + // locked mutex group. This is an error so let's release it. + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(assignment.group) + .claimer(requester_id()) + .claim_time(assignment.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + } } } +//============================================================================== +void RobotContext::_release_mutex_group(MutexGroupData& data) +{ + if (data.name.empty()) + { + return; + } + + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(data.name) + .claimer(requester_id()) + .claim_time(data.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + data.name = ""; +} + //============================================================================== void RobotContext::_publish_mutex_group_request() { @@ -1287,27 +1302,52 @@ void RobotContext::_publish_mutex_group_request() { // The robot has been idle for 10 seconds. It should not be keeping a // mutex locked; a task probably ended wrongly. - if (!_mutex_group.empty()) + if (!_requesting_mutex_group.name.empty() + || !_locked_mutex_group.name.empty()) { - 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.", - _mutex_group.c_str(), - requester_id().c_str()); - _mutex_group.clear(); + 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()); + }; + + if (!_requesting_mutex_group.name.empty()) + { + warning(_requesting_mutex_group.name); + } + + if (!_locked_mutex_group.name.empty()) + { + warning(_locked_mutex_group.name); + } + + release_mutex_group(); } } } - if (!_mutex_group.empty()) + auto publish = [&](const MutexGroupData& data) + { + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(data.name) + .claimer(requester_id()) + .claim_time(data.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); + }; + + if (!_requesting_mutex_group.name.empty()) + { + publish(_requesting_mutex_group); + } + + if (!_locked_mutex_group.name.empty()) { - _node->mutex_group_request()->publish( - rmf_fleet_msgs::build() - .group(_mutex_group) - .claimer(requester_id()) - .claim_time(_mutex_group_claim_time) - .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); + publish(_locked_mutex_group); } } 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 dd420e009..c3e9f4209 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -289,6 +289,13 @@ struct LiftDestination bool requested_from_inside; }; +//============================================================================== +struct MutexGroupData +{ + std::string name; + builtin_interfaces::msg::Time claim_time; +}; + //============================================================================== class RobotContext : public std::enable_shared_from_this, @@ -561,13 +568,10 @@ class RobotContext void release_lift(); /// What mutex group is currently being locked. - const std::string& current_mutex_group() const; - - /// Have we obtained the current mutex group yet? - bool obtained_mutex_group() const; + const std::string& locked_mutex_group() const; /// Set the mutex group that this robot needs to have locked. - void set_mutex_group(std::string group); + void request_mutex_group(std::string group); /// If we are holding a mutex group, release it void release_mutex_group(); @@ -716,10 +720,10 @@ class RobotContext std::optional _initial_time_idle_outside_lift; void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); + void _release_mutex_group(MutexGroupData& data); void _publish_mutex_group_request(); - std::string _mutex_group; - bool _obtained_mutex_group = false; - builtin_interfaces::msg::Time _mutex_group_claim_time; + MutexGroupData _requesting_mutex_group; + MutexGroupData _locked_mutex_group; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; std::chrono::steady_clock::time_point _last_active_task_time; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index e8606498f..151b677a2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -127,24 +127,21 @@ void LockMutexGroup::Active::_initialize() using MutexGroupStatesPtr = std::shared_ptr; - if (_context->current_mutex_group() == _data.mutex_group) + if (_context->locked_mutex_group() == _data.mutex_group) { - if (_context->obtained_mutex_group()) - { - RCLCPP_INFO( - _context->node()->get_logger(), - "Mutex group [%s] was already locked for [%s]", - _data.mutex_group.c_str(), - _context->requester_id().c_str()); - // We don't need to do anything further, we already got the mutex group - // previously. - _context->worker().schedule( - [finished = _finished](const auto&) - { - finished(); - }); - return; - } + RCLCPP_INFO( + _context->node()->get_logger(), + "Mutex group [%s] was already locked for [%s]", + _data.mutex_group.c_str(), + _context->requester_id().c_str()); + // We don't need to do anything further, we already got the mutex group + // previously. + _context->worker().schedule( + [finished = _finished](const auto&) + { + finished(); + }); + return; } _state->update_log().info( @@ -155,6 +152,7 @@ void LockMutexGroup::Active::_initialize() _data.mutex_group.c_str(), _context->requester_id().c_str()); + _stubborn = _context->be_stubborn(); const auto t_buffer = std::chrono::seconds(10); const auto zero = Eigen::Vector3d::Zero(); rmf_traffic::Trajectory hold_traj; @@ -217,7 +215,7 @@ void LockMutexGroup::Active::_initialize() std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() << " TO " << _data.mutex_group << std::endl; - _context->set_mutex_group(_data.mutex_group); + _context->request_mutex_group(_data.mutex_group); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp index eee2acec3..d1e2169f0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -94,6 +94,7 @@ class LockMutexGroup : public rmf_task_sequence::Event std::function _finished; rmf_rxcpp::subscription_guard _listener; rclcpp::TimerBase::SharedPtr _delay_timer; + std::shared_ptr _stubborn; Data _data; }; }; 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 5a02ecf94..c26c81778 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -282,7 +282,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) } } - if (!context->current_mutex_group().empty()) + if (!context->locked_mutex_group().empty()) { const auto adjusted_now = now - new_cumulative_delay; const auto& graph = context->navigation_graph(); From dc32398139ff78ea7d3f2a6cba5900608a0b07da Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 16 Nov 2023 05:08:03 +0000 Subject: [PATCH 54/89] Fix move_through continuity error Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 4 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 40 +++++++++++++++++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 12 ++++++ 3 files changed, 54 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 8141d3151..28a492ca6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1070,7 +1070,7 @@ void EasyCommandHandle::follow_new_path( const auto& i_wp1 = lane.exit().waypoint_index(); const auto& wp1 = graph.get_waypoint(i_wp1); ss << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() - << "] -> " << i_wp1 << "[" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + << "] -> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; if (lane.exit().event()) { ss << "[exit "; @@ -1090,7 +1090,7 @@ void EasyCommandHandle::follow_new_path( std::cout << context->requester_id() << " Locations: " << current_location.size() << std::endl; for (const auto& l : current_location) { - std::cout << " -- xyz " << print_location(l) << std::endl; + std::cout << " -- " << print_location(l) << std::endl; } std::cout << "Num waypoints: " << waypoints.size() << std::endl; for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) 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 c3e9f4209..9533962f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -94,6 +94,46 @@ inline std::string print_waypoint( return ss.str(); } +//============================================================================== +inline std::string print_plan_waypoint( + const rmf_traffic::agv::Plan::Waypoint& wp, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + ss << "t=" << rmf_traffic::time::to_seconds(wp.time().time_since_epoch()); + 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 << "]"; + } + } + + for (const auto& l : wp.approach_lanes()) + { + const auto& lane = graph.get_lane(l); + const auto& m = lane.properties().in_mutex_group(); + if (!m.empty()) + { + ss << " lane mutex [" << m << "]"; + } + } + + return ss.str(); +} + //============================================================================== inline std::string print_lane_node( const rmf_traffic::agv::Graph::Lane::Node& node, 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 d9fbaff27..e2691a23a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -533,6 +533,14 @@ std::optional ExecutePlan::make( std::vector waypoints = plan.get_waypoints(); + std::stringstream pss; + pss << "Plan waypoints"; + for (const auto& wp : waypoints) + { + pss << "\n -- " << agv::print_plan_waypoint(wp, graph); + } + std::cout << pss.str() << std::endl; + std::vector move_through; std::optional current_mutex_group; @@ -652,7 +660,11 @@ std::optional ExecutePlan::make( move_through.back().time(), move_through.back().dependencies(), current_mutex_group); + 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_group = next_mutex_group; From e70f618469191186c6fb504af199bcd016d7fb39 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 16 Nov 2023 06:30:01 +0000 Subject: [PATCH 55/89] Fix atomic switching Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 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 56dfd04a0..0ea519c93 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1242,7 +1242,7 @@ void RobotContext::_check_mutex_groups( if (assignment.group == _requesting_mutex_group.name) { - if (_locked_mutex_group.name != _requesting_mutex_group.name) + if (_locked_mutex_group.name != assignment.group) { _node->mutex_group_request()->publish( rmf_fleet_msgs::build() @@ -1250,14 +1250,11 @@ void RobotContext::_check_mutex_groups( .claimer(requester_id()) .claim_time(_locked_mutex_group.claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); - - _locked_mutex_group = _requesting_mutex_group; } - return; + _locked_mutex_group = _requesting_mutex_group; } - - if (assignment.group != _locked_mutex_group.name) + else if (assignment.group != _locked_mutex_group.name) { // This assignment does not match either the requested nor the currently // locked mutex group. This is an error so let's release it. From 7f7923120cb7f17c07c39ec11cfe331ac349d1e0 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 17 Nov 2023 04:02:04 +0000 Subject: [PATCH 56/89] Ignore traffic dependencies for agents waiting on your locked mutex group Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/StandardNames.hpp | 2 + .../src/mutex_group_supervisor/main.cpp | 36 +++++------ .../src/rmf_fleet_adapter/agv/Node.cpp | 18 ++++-- .../src/rmf_fleet_adapter/agv/Node.hpp | 4 ++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 26 +++++--- .../rmf_fleet_adapter/agv/RobotContext.hpp | 7 ++- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 9 --- .../events/LockMutexGroup.cpp | 14 +---- .../events/WaitForTraffic.cpp | 60 +++++++++++++++++-- .../events/WaitForTraffic.hpp | 1 + .../rmf_fleet_adapter/phases/MoveRobot.hpp | 5 ++ .../schedule/MirrorManager.cpp | 20 +++++++ .../src/rmf_traffic_ros2/schedule/Node.cpp | 27 +++++++++ .../schedule/internal_Node.hpp | 2 + 14 files changed, 176 insertions(+), 55 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index c3a94086c..a736ce9e5 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 uint64_t Unclaimed = (uint64_t)(-1); + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index 9967b387d..a7856f5bc 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -34,11 +34,13 @@ struct TimeStamps std::chrono::steady_clock::time_point heartbeat_time; }; -using ClaimMap = std::unordered_map; +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: @@ -78,7 +80,7 @@ class Node : public rclcpp::Node const auto g_it = mutex_groups.find(request.group); if (g_it != mutex_groups.end()) { - const auto c_it = g_it->second.find(request.claimer); + 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) @@ -95,10 +97,10 @@ class Node : public rclcpp::Node 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.claimer, timestamps); + claims.insert_or_assign(request.claimant, timestamps); for (const auto& s : latest_states.assignments) { - if (s.group == request.group && !s.claimed.empty()) + if (s.group == request.group && s.claimant != Unclaimed) { // The group is already claimed, so nothing to be done here return; @@ -114,21 +116,21 @@ class Node : public rclcpp::Node const auto timeout = std::chrono::seconds(10); for (auto& [group, claims] : mutex_groups) { - std::vector remove_claims; - for (const auto& [claimer, timestamp] : claims) + std::vector remove_claims; + for (const auto& [claimant, timestamp] : claims) { if (timestamp.heartbeat_time + timeout < now) { - remove_claims.push_back(claimer); + remove_claims.push_back(claimant); } } - std::string current_claimer; + uint64_t current_claimer = Unclaimed; for (const auto& assignment : latest_states.assignments) { if (assignment.group == group) { - current_claimer = assignment.claimed; + current_claimer = assignment.claimant; break; } } @@ -155,30 +157,30 @@ class Node : public rclcpp::Node void pick_next(const std::string& group) { - const auto& claimers = mutex_groups[group]; - std::optional> earliest; - for (const auto& [claimer, timestamp] : claimers) + 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, claimer); + earliest = std::make_pair(t, claimant); } } - std::string claimer; + uint64_t claimant = Unclaimed; builtin_interfaces::msg::Time claim_time; if (earliest.has_value()) { claim_time = earliest->first; - claimer = earliest->second; + claimant = earliest->second; } bool group_found = false; for (auto& a : latest_states.assignments) { if (a.group == group) { - a.claimed = claimer; + a.claimant = claimant; a.claim_time = claim_time; group_found = true; break; @@ -189,7 +191,7 @@ class Node : public rclcpp::Node latest_states.assignments.push_back( rmf_fleet_msgs::build() .group(group) - .claimed(claimer) + .claimant(claimant) .claim_time(claim_time)); } } 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 000da0819..322281326 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -108,14 +108,18 @@ std::shared_ptr Node::make( node->create_publisher( TaskApiResponses, transient_local_qos); - node->_mutex_group_states_obs = - node->create_observable( - MutexGroupStatesTopicName, 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; } @@ -245,6 +249,12 @@ 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& { 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 26be6c875..5331b43e2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -128,6 +128,9 @@ class Node : public rmf_rxcpp::Transport 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; @@ -190,6 +193,7 @@ class Node : public rmf_rxcpp::Transport 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; }; 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 0ea519c93..86326df92 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -565,6 +565,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 { @@ -1032,7 +1038,9 @@ const std::string& RobotContext::locked_mutex_group() const } //============================================================================== -void RobotContext::request_mutex_group(std::string group) +void RobotContext::request_mutex_group( + std::string group, + rmf_traffic::Time claim_time) { if (group.empty()) { @@ -1045,7 +1053,11 @@ void RobotContext::request_mutex_group(std::string group) return; } - _requesting_mutex_group = MutexGroupData{std::move(group), _node->now()}; + _requesting_mutex_group = MutexGroupData{ + std::move(group), + rmf_traffic_ros2::convert(claim_time) + }; + _publish_mutex_group_request(); } @@ -1237,7 +1249,7 @@ void RobotContext::_check_mutex_groups( // lock right now. for (const auto& assignment : states.assignments) { - if (assignment.claimed != requester_id()) + if (assignment.claimant != participant_id()) return; if (assignment.group == _requesting_mutex_group.name) @@ -1247,7 +1259,7 @@ void RobotContext::_check_mutex_groups( _node->mutex_group_request()->publish( rmf_fleet_msgs::build() .group(_locked_mutex_group.name) - .claimer(requester_id()) + .claimant(participant_id()) .claim_time(_locked_mutex_group.claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); } @@ -1261,7 +1273,7 @@ void RobotContext::_check_mutex_groups( _node->mutex_group_request()->publish( rmf_fleet_msgs::build() .group(assignment.group) - .claimer(requester_id()) + .claimant(participant_id()) .claim_time(assignment.claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); } @@ -1279,7 +1291,7 @@ void RobotContext::_release_mutex_group(MutexGroupData& data) _node->mutex_group_request()->publish( rmf_fleet_msgs::build() .group(data.name) - .claimer(requester_id()) + .claimant(participant_id()) .claim_time(data.claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); data.name = ""; @@ -1332,7 +1344,7 @@ void RobotContext::_publish_mutex_group_request() _node->mutex_group_request()->publish( rmf_fleet_msgs::build() .group(data.name) - .claimer(requester_id()) + .claimant(participant_id()) .claim_time(data.claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); }; 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 9533962f3..c722e657a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -411,6 +411,9 @@ 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; @@ -611,7 +614,9 @@ class RobotContext const std::string& locked_mutex_group() const; /// Set the mutex group that this robot needs to have locked. - void request_mutex_group(std::string group); + void request_mutex_group( + std::string group, + rmf_traffic::Time claim_time); /// If we are holding a mutex group, release it void release_mutex_group(); 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 e2691a23a..8f6bdb6c6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -643,15 +643,6 @@ std::optional ExecutePlan::make( if (move_through.size() > 1) { auto next_mutex_group = make_current_mutex_group(move_through.back()); - std::size_t excluded_route = 0; - for (const auto& c : move_through.back().arrival_checkpoints()) - { - excluded_route = std::max(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()); - } - previous_itinerary->erase(previous_itinerary->begin()+excluded_route); std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; legacy_phases.emplace_back( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index 151b677a2..ba85d2819 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -152,15 +152,6 @@ void LockMutexGroup::Active::_initialize() _data.mutex_group.c_str(), _context->requester_id().c_str()); - _stubborn = _context->be_stubborn(); - const auto t_buffer = std::chrono::seconds(10); - const auto zero = Eigen::Vector3d::Zero(); - rmf_traffic::Trajectory hold_traj; - hold_traj.insert(_data.hold_time, _data.hold_position, zero); - hold_traj.insert(_data.hold_time + t_buffer, _data.hold_position, zero); - rmf_traffic::Route hold(_data.hold_map, std::move(hold_traj)); - _schedule({hold}); - const auto cumulative_delay = _context->now() - _data.hold_time; _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); @@ -178,14 +169,13 @@ void LockMutexGroup::Active::_initialize() { if (assignment.group == self->_data.mutex_group) { - if (assignment.claimed == self->_context->requester_id()) + if (assignment.claimant == self->_context->participant_id()) { const auto finished = self->_finished; self->_finished = nullptr; if (finished) { std::cout << " === FINISHED " << __LINE__ << std::endl; - self->_schedule(*self->_data.resume_itinerary); self->_state->update_status(State::Status::Completed); RCLCPP_INFO( self->_context->node()->get_logger(), @@ -215,7 +205,7 @@ void LockMutexGroup::Active::_initialize() std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() << " TO " << _data.mutex_group << std::endl; - _context->request_mutex_group(_data.mutex_group); + _context->request_mutex_group(_data.mutex_group, _data.hold_time); } //============================================================================== 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 7b1a84aed..532545512 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -98,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; @@ -106,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 (msg->group == self->_context->locked_mutex_group()) + { + // 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()) @@ -229,16 +263,32 @@ void WaitForTraffic::Active::_consider_going() } bool all_dependencies_reached = true; + std::stringstream ss; for (const auto& dep : _dependencies) { - const auto current = _context->schedule()->get_current_plan_id(dep.dependency().on_participant); - std::cout << " -- " << dep.dependency().on_participant << ":" << dep.dependency().on_plan - << " vs current " << current.value_or((std::size_t)(-1)) - << " | " << dep.reached() << " | " << dep.deprecated() - << std::endl; + const auto& d = dep.dependency(); + const auto current = _context->schedule()->get_current_plan_id(d.on_participant); + const auto* p = _context->schedule()->get_current_progress(d.on_participant); + ss << " -- " << d.on_participant << " | " << d.on_plan + << ":" << d.on_route << ":" << d.on_checkpoint + << " vs current " << current.value_or((std::size_t)(-1)); + if (p) + { + for (std::size_t i=0; i < p->size(); ++i) + { + ss << ":[" << i << ":" << (*p)[i] << "]"; + } + } + else + { + ss << "null"; + } + + ss << " | " << dep.reached() << " | " << dep.deprecated() << "\n"; if (!dep.reached() && !dep.deprecated()) all_dependencies_reached = false; } + std::cout << ss.str() << std::endl; if (all_dependencies_reached) { 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 43f432fba..d4651c2be 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp @@ -105,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/MoveRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp index c26c81778..c1ec798ba 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -333,6 +333,11 @@ void MoveRobot::Action::operator()(const Subscriber& s) { self->_context->itinerary().reached( self->_plan_id, c.route_id, c.checkpoint_id); + + std::cout << "finish reached " << self->_context->participant_id() + << " | " << self->_context->itinerary().current_plan_id() + << ":" << c.route_id << ":" << c.checkpoint_id + << " #" << self->_context->itinerary().progress_version() << std::endl; } const auto last_index = self->_waypoints.back().graph_index(); 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..ddc291738 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp @@ -396,6 +396,26 @@ class MirrorManager::Implementation try { const rmf_traffic::schedule::Patch patch = convert(msg->patch); + std::stringstream ss; + bool new_progress = false; + for (const auto& c : patch) + { + if (c.progress().has_value()) + { + new_progress = true; + const auto& progress = c.progress()->checkpoints(); + ss << "\n -- #" << c.progress()->version() << " " << c.participant_id() << " |"; + for (std::size_t i=0; i < progress.size(); ++i) + { + ss << " " << i << ":" << progress[i]; + } + } + } + if (new_progress) + { + ss << "\n >>>>>> NEW PROGRESS"; + std::cout << ss.str() << std::endl; + } std::mutex* update_mutex = options.update_mutex(); if (update_mutex) 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..fba0f2c17 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp @@ -287,6 +287,33 @@ void ScheduleNode::setup(const QueryMap& queries) setup_incosistency_pub(); setup_conflict_topics_and_thread(); setup_cull_timer(); + + debug_timer = create_wall_timer( + std::chrono::seconds(2), + [&]() + { + std::stringstream ss; + ss << " ==== PROGRESS"; + for (const auto id : database->participant_ids()) + { + ss << "\n -- #" << database->get_current_progress_version(id) << " " << id << " | plan " + << database->get_current_plan_id(id).value_or((std::size_t)(-1)); + const auto* p = database->get_current_progress(id); + if (p) + { + for (std::size_t i=0; i < p->size(); ++i) + { + ss << " " << i << ":" << (*p)[i]; + } + } + else + { + ss << "null"; + } + } + + std::cout << ss.str() << std::endl; + }); } //============================================================================== diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp index c5c7a7ac1..153082bf0 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp @@ -544,6 +544,8 @@ class ScheduleNode : public rclcpp::Node // description versions separately from itinerary versions. std::size_t last_known_participants_version = 0; std::size_t current_participants_version = 1; + + rclcpp::TimerBase::SharedPtr debug_timer; }; } // namespace schedule From aa624543c30e206288e28e74094c2f665b134d75 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 20 Nov 2023 02:11:51 +0000 Subject: [PATCH 57/89] Use mutex group switches as a horizon Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 107 ++++++++++++------ .../events/LockMutexGroup.cpp | 13 ++- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 3 + 3 files changed, 86 insertions(+), 37 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 8f6bdb6c6..8aac2da4b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -628,9 +628,43 @@ std::optional ExecutePlan::make( } } + 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(); + std::stringstream ss; + ss << "erasing up to checkpoint " << c.checkpoint_id << "\n"; + ss << "t.size() before: " << t.size(); + t.erase(t.begin() + (int)c.checkpoint_id, t.end()); + ss << " | after: " << t.size(); + std::cout << ss.str() << std::endl; + } + + 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()); + auto next_itinerary = std::make_shared< rmf_traffic::schedule::Itinerary>(full_itinerary); - return LockMutexGroup::Data{ + auto data = LockMutexGroup::Data{ new_mutex_group, hold_map, hold_position, @@ -638,6 +672,10 @@ std::optional ExecutePlan::make( plan_id, next_itinerary }; + + previous_itinerary = data.resume_itinerary; + + return data; }; if (move_through.size() > 1) @@ -660,8 +698,6 @@ std::optional ExecutePlan::make( current_mutex_group = next_mutex_group; - previous_itinerary = current_mutex_group->resume_itinerary; - // We treat this the same as an event occurring to indicate that // we should keep looping. event_occurred = true; @@ -674,7 +710,6 @@ std::optional ExecutePlan::make( if (move_through.empty()) { current_mutex_group = make_current_mutex_group(*it); - previous_itinerary = current_mutex_group->resume_itinerary; std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; } else @@ -682,7 +717,6 @@ std::optional ExecutePlan::make( assert(move_through.size() == 1); current_mutex_group = make_current_mutex_group( move_through.back()); - previous_itinerary = current_mutex_group->resume_itinerary; std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; } } @@ -873,43 +907,46 @@ std::optional ExecutePlan::make( } std::size_t attempts = 0; - while (!context->itinerary().set(*plan_id, *initial_itinerary)) + if (!initial_itinerary->empty()) { - // 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 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()); - 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) + 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]", + "Invalid 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) + { + RCLCPP_ERROR( + context->node()->get_logger(), + "Requesting replan for [%s] in group [%s] because plan is repeatedly " + "being rejected while performing task [%s]", + context->name().c_str(), + context->group().c_str(), + task_id.c_str()); + return std::nullopt; + } } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index ba85d2819..f7e9a365f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -134,6 +134,8 @@ void LockMutexGroup::Active::_initialize() "Mutex group [%s] was already locked for [%s]", _data.mutex_group.c_str(), _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( @@ -152,7 +154,8 @@ void LockMutexGroup::Active::_initialize() _data.mutex_group.c_str(), _context->requester_id().c_str()); - const auto cumulative_delay = _context->now() - _data.hold_time; + const auto cumulative_delay = _context->now() - _data.hold_time + - std::chrono::seconds(2); _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); _listener = _context->node()->mutex_group_states() @@ -175,7 +178,13 @@ void LockMutexGroup::Active::_initialize() self->_finished = nullptr; if (finished) { - std::cout << " === FINISHED " << __LINE__ << std::endl; + if (!self->_data.resume_itinerary->empty()) + { + self->_schedule(*self->_data.resume_itinerary); + } + std::cout << " === LOCKED MUTEX " << __LINE__ << " new plan id for " + << self->_context->requester_id() << ": " + << *self->_data.plan_id << std::endl; self->_state->update_status(State::Status::Completed); RCLCPP_INFO( self->_context->node()->get_logger(), 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 c1ec798ba..bebc51a54 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -258,6 +258,9 @@ void MoveRobot::Action::operator()(const Subscriber& s) const auto self = w.lock(); if (!self) return; + std::cout << "delaying " << self->_context->requester_id() << " plan id " + << self->_plan_id << " by " << rmf_traffic::time::to_seconds(new_cumulative_delay) + << std::endl; const auto context = self->_context; const auto plan_id = self->_plan_id; context->itinerary().cumulative_delay( From 5eec5f2d0cbdc399ffcc997cf65976e8ba65f8c1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 20 Nov 2023 05:12:20 +0000 Subject: [PATCH 58/89] Update to latest API Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 2 +- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 16 +++++----------- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 13 ++++++------- 3 files changed, 12 insertions(+), 19 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 28a492ca6..b0b9d04e9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1307,7 +1307,7 @@ void EasyCommandHandle::follow_new_path( } else { - for (const auto& lift : graph.known_lifts()) + for (const auto& lift : graph.all_known_lifts()) { if (lift->is_in_lift(target_position.block<2, 1>(0, 0))) { 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 86326df92..8bc0a566c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -243,7 +243,7 @@ rmf_traffic::agv::Plan::StartSet NavParams::_lift_boundary_filter( const auto robot_inside_lift = [&]() -> rmf_traffic::agv::Graph::LiftPropertiesPtr { - for (const auto& lift : graph.known_lifts()) + for (const auto& lift : graph.all_known_lifts()) { // We assume lifts never overlap so we will return the first // positive hit. @@ -1168,19 +1168,13 @@ void RobotContext::_check_lift_state( { const Eigen::Vector2d p = position().block<2, 1>(0, 0); const auto& graph = navigation_graph(); - const auto& known_lifts = graph.known_lifts(); - const auto l_it = std::find_if( - known_lifts.begin(), - known_lifts.end(), - [&](const auto& lift) - { - return lift->name() == _lift_destination->lift_name; - }); + const auto found_lift = + graph.find_known_lift(_lift_destination->lift_name); bool inside_lift = false; - if (l_it != graph.known_lifts().end()) + if (found_lift) { - inside_lift = (*l_it)->is_in_lift(p); + inside_lift = found_lift->is_in_lift(p); } if (inside_lift) 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 37ca222b1..22b859bdf 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 @@ -37,7 +37,7 @@ rmf_traffic::agv::Graph parse_graph( throw std::runtime_error("Failed to load graph file [" + graph_file + "]"); } - std::unordered_map lifts; + rmf_traffic::agv::Graph graph; bool has_lifts = false; const YAML::Node lifts_yaml = graph_config["lifts"]; if (!lifts_yaml) @@ -67,8 +67,8 @@ rmf_traffic::agv::Graph parse_graph( dims_yaml[0].as(), dims_yaml[1].as()); - lifts[name] = std::make_shared( - name, location, orientation, dimensions); + graph.set_known_lift(rmf_traffic::agv::Graph::LiftProperties( + name, location, orientation, dimensions)); } } @@ -95,7 +95,6 @@ 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; @@ -181,15 +180,15 @@ rmf_traffic::agv::Graph parse_graph( lift_of_wp[wp.index()] = lift_name; if (has_lifts) { - const auto l_it = lifts.find(lift_name); - if (l_it == lifts.end()) + 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(l_it->second); + wp.set_in_lift(lift); } } } From 55c62b119105b699420e606e0ea029cb180daea6 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 20 Nov 2023 15:06:49 +0000 Subject: [PATCH 59/89] Fix cases where a mutex group needs to be locked immediately before an event Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 46 +++++++++++++------ .../events/LockMutexGroup.cpp | 3 ++ .../events/WaitForTraffic.cpp | 1 + .../rmf_fleet_adapter/phases/DockRobot.cpp | 30 ++++++++++-- .../rmf_fleet_adapter/phases/DockRobot.hpp | 24 ++++++++-- 5 files changed, 83 insertions(+), 21 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 8aac2da4b..45d6c0152 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -110,12 +110,14 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor LegacyPhases& phases, const rmf_traffic::agv::Plan::Waypoint& waypoint_, const PlanIdPtr plan_id, + std::optional current_mutex_group, bool& continuous) : waypoint(waypoint_), _context(std::move(context)), _phases(phases), _event_start_time(waypoint_.time()), _plan_id(plan_id), + _current_mutex_group(std::move(current_mutex_group)), _continuous(continuous) { // Do nothing @@ -128,8 +130,8 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor assert(!_moving_lift); _phases.emplace_back( std::make_shared( - _context, dock.dock_name()), - _event_start_time, waypoint.dependencies(), std::nullopt); + _context, dock.dock_name(), waypoint, _plan_id), + _event_start_time, waypoint.dependencies(), _current_mutex_group); _continuous = false; } @@ -143,7 +145,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.name(), _context->requester_id(), _event_start_time + open.duration()), - _event_start_time, waypoint.dependencies(), std::nullopt); + _event_start_time, waypoint.dependencies(), _current_mutex_group); _continuous = true; } @@ -158,7 +160,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, waypoint.dependencies(), std::nullopt); + _event_start_time, waypoint.dependencies(), _current_mutex_group); _continuous = true; } @@ -174,7 +176,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _event_start_time, phases::RequestLift::Located::Outside, _plan_id), - _event_start_time, waypoint.dependencies(), std::nullopt); + _event_start_time, waypoint.dependencies(), _current_mutex_group); _continuous = true; } @@ -209,7 +211,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor phases::RequestLift::Located::Inside, _plan_id, localize), - _event_start_time, waypoint.dependencies(), std::nullopt); + _event_start_time, waypoint.dependencies(), _current_mutex_group); _moving_lift = false; _continuous = true; @@ -224,7 +226,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, waypoint.dependencies(), std::nullopt); + _event_start_time, waypoint.dependencies(), _current_mutex_group); _continuous = true; } @@ -244,6 +246,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor LegacyPhases& _phases; rmf_traffic::Time _event_start_time; PlanIdPtr _plan_id; + std::optional _current_mutex_group; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); @@ -629,18 +632,28 @@ std::optional ExecutePlan::make( } std::size_t first_excluded_route = 0; + std::stringstream ss; + if (current_mutex_group.has_value()) + { + ss << "truncating for switch from [" + << current_mutex_group->mutex_group + << "] to [" << new_mutex_group << "]"; + } + else + { + ss << "truncating to lock [" << new_mutex_group << "]"; + } 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(); - std::stringstream ss; - ss << "erasing up to checkpoint " << c.checkpoint_id << "\n"; + ss << "\n -- erasing up to checkpoint " << c.checkpoint_id << "\n"; ss << "t.size() before: " << t.size(); t.erase(t.begin() + (int)c.checkpoint_id, t.end()); ss << " | after: " << t.size(); - std::cout << ss.str() << std::endl; } + std::cout << ss.str() << std::endl; for (std::size_t i=0; i < previous_itinerary->size(); ++i) { @@ -731,6 +744,7 @@ std::optional ExecutePlan::make( if (it->event()) { + std::optional event_mutex_group; if (move_through.size() > 1) { legacy_phases.emplace_back( @@ -738,11 +752,16 @@ std::optional ExecutePlan::make( context, move_through, plan_id, tail_period), it->time(), it->dependencies(), current_mutex_group); } + else if(mutex_group_change) + { + std::cout << " >>>> appending mutex group to event" << std::endl; + event_mutex_group = current_mutex_group; + } move_through.clear(); bool continuous = true; EventPhaseFactory factory( - context, legacy_phases, *it, plan_id, continuous); + context, legacy_phases, *it, plan_id, event_mutex_group, continuous); it->event()->execute(factory); while (factory.moving_lift()) { @@ -857,7 +876,7 @@ std::optional ExecutePlan::make( { if (head->mutex_group_dependency.has_value()) { - ss << "\n -- Adding mutex group [" << head->mutex_group_dependency->mutex_group << "]"; + ss << "\n -- Lock mutex group [" << head->mutex_group_dependency->mutex_group << "]"; standbys.push_back(make_wait_for_mutex( context, event_id, head->mutex_group_dependency.value())); } @@ -887,9 +906,8 @@ std::optional ExecutePlan::make( ++head; } - - std::cout << ss.str() << std::endl; } + std::cout << ss.str() << std::endl; if (tail_period.has_value() && !legacy_phases.empty()) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index f7e9a365f..e9af5dce0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -221,6 +221,9 @@ void LockMutexGroup::Active::_initialize() void LockMutexGroup::Active::_schedule( rmf_traffic::schedule::Itinerary itinerary) const { + std::cout << " --- [" << _context->requester_id() << "] resuming with " + << itinerary.size() << " routes" << std::endl; + bool scheduled = false; std::size_t attempts = 0; while (!scheduled) 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 532545512..84cfc61fe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -264,6 +264,7 @@ void WaitForTraffic::Active::_consider_going() bool all_dependencies_reached = true; std::stringstream ss; + ss << "consider going for [" << _context->requester_id() << "]\n"; for (const auto& dep : _dependencies) { const auto& d = dep.dependency(); 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..1ff2a9628 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp @@ -23,9 +23,13 @@ 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; @@ -73,9 +77,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 +93,21 @@ 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()); + } + return std::make_shared( + _context, _dock_name, _waypoint, plan_id); } //============================================================================== 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..647cc5b39 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -35,7 +35,9 @@ struct DockRobot 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; @@ -53,6 +55,8 @@ struct DockRobot agv::RobotContextPtr _context; std::string _dock_name; std::string _description; + rmf_traffic::agv::Plan::Waypoint _waypoint; + rmf_traffic::PlanId _plan_id; std::shared_ptr _action; rxcpp::observable _obs; std::shared_ptr _be_stubborn; @@ -64,7 +68,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,6 +83,8 @@ struct DockRobot agv::RobotContextPtr _context; std::string _dock_name; std::string _description; + rmf_traffic::agv::Plan::Waypoint _waypoint; + PlanIdPtr _plan_id; }; class Action @@ -106,12 +114,22 @@ void DockRobot::Action::operator()(const Subscriber& s) s.on_next(status); _phase->_context->command()->dock( _phase->_dock_name, - [s, dock_name = _phase->_dock_name, context = _phase->_context]() + [s, dock_name = _phase->_dock_name, context = _phase->_context, + wp = _phase->_waypoint, plan_id = _phase->_plan_id]() { 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()) + { + std::cout << "docking reached " << context->participant_id() + << " | " << context->itinerary().current_plan_id() + << ":" << c.route_id << ":" << c.checkpoint_id + << " #" << context->itinerary().progress_version() << std::endl; + context->itinerary().reached(plan_id, c.route_id, c.checkpoint_id); + } + s.on_next(status); s.on_completed(); }); From 485d6f35385df3cf47063a64bc2a8c9c9588620b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 20 Nov 2023 18:08:32 +0000 Subject: [PATCH 60/89] Lock mutex group before docking when needed Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 4 +- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 1 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 322 +++++++++++------- 3 files changed, 197 insertions(+), 130 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 28a492ca6..e142bc729 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1282,7 +1282,9 @@ void EasyCommandHandle::follow_new_path( const auto& wp2 = waypoints[i2]; if (wp1.graph_index().has_value() && wp2.graph_index().has_value()) { - if (*wp1.graph_index() == *wp2.graph_index()) + const auto gi_1 = *wp1.graph_index(); + const auto gi_2 = *wp2.graph_index(); + if (nav_params->in_same_stack(gi_1, gi_2)) { target_index = i2; target_position = wp2.position(); 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 37ca222b1..56e80cdd0 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 @@ -337,6 +337,7 @@ rmf_traffic::agv::Graph parse_graph( // 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()); graph.add_lane( {begin, entry_event}, 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 45d6c0152..f2f0f229d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -108,30 +108,63 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor EventPhaseFactory( agv::RobotContextPtr context, LegacyPhases& phases, - const rmf_traffic::agv::Plan::Waypoint& waypoint_, + const rmf_traffic::agv::Plan::Waypoint& initial_waypoint_, + std::optional next_waypoint_, const PlanIdPtr plan_id, - std::optional current_mutex_group, + std::optional& current_mutex_group, + std::function make_current_mutex_group, + std::function( + const rmf_traffic::agv::Plan::Waypoint&)> get_new_mutex_group, bool& continuous) - : waypoint(waypoint_), + : initial_waypoint(initial_waypoint_), + next_waypoint(std::move(next_waypoint_)), _context(std::move(context)), _phases(phases), - _event_start_time(waypoint_.time()), + _event_start_time(initial_waypoint_.time()), _plan_id(plan_id), - _current_mutex_group(std::move(current_mutex_group)), + _current_mutex_group(current_mutex_group), + _make_current_mutex_group(std::move(make_current_mutex_group)), + _get_new_mutex_group(std::move(get_new_mutex_group)), _continuous(continuous) { // Do nothing } - rmf_traffic::agv::Plan::Waypoint waypoint; + 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_group] = + _get_new_mutex_group(*next_waypoint); + + if (mutex_group_change) + { + if (!new_mutex_group.empty()) + { + event_mutex_group = _make_current_mutex_group( + new_mutex_group, initial_waypoint); + _current_mutex_group = event_mutex_group; + } + else + { + _current_mutex_group = std::nullopt; + } + } + } + assert(!_moving_lift); _phases.emplace_back( std::make_shared( - _context, dock.dock_name(), waypoint, _plan_id), - _event_start_time, waypoint.dependencies(), _current_mutex_group); + _context, dock.dock_name(), + next_waypoint.value_or(initial_waypoint), + _plan_id), + _event_start_time, initial_waypoint.dependencies(), event_mutex_group); _continuous = false; } @@ -145,7 +178,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.name(), _context->requester_id(), _event_start_time + open.duration()), - _event_start_time, waypoint.dependencies(), _current_mutex_group); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -160,7 +193,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, waypoint.dependencies(), _current_mutex_group); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -176,7 +209,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _event_start_time, phases::RequestLift::Located::Outside, _plan_id), - _event_start_time, waypoint.dependencies(), _current_mutex_group); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -196,8 +229,8 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor const auto node = _context->node(); auto localize = agv::Destination::Implementation::make( open.floor_name(), - waypoint.position(), - waypoint.graph_index(), + initial_waypoint.position(), + initial_waypoint.graph_index(), std::nullopt, nullptr); @@ -211,7 +244,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor phases::RequestLift::Located::Inside, _plan_id, localize), - _event_start_time, waypoint.dependencies(), _current_mutex_group); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _moving_lift = false; _continuous = true; @@ -226,7 +259,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, waypoint.dependencies(), _current_mutex_group); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -246,7 +279,12 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor LegacyPhases& _phases; rmf_traffic::Time _event_start_time; PlanIdPtr _plan_id; - std::optional _current_mutex_group; + std::optional& _current_mutex_group; + std::function _make_current_mutex_group; + std::function( + const rmf_traffic::agv::Plan::Waypoint&)> _get_new_mutex_group; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); @@ -547,18 +585,121 @@ std::optional ExecutePlan::make( std::vector move_through; std::optional current_mutex_group; - LegacyPhases legacy_phases; - while (!waypoints.empty()) + const auto make_current_mutex_group = [&]( + const std::string& new_mutex_group, + const rmf_traffic::agv::Plan::Waypoint& wp) { - auto it = waypoints.begin(); - bool event_occurred = false; - for (; it != waypoints.end(); ++it) + 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 [%s] transition needed " + "by robot [%s]. There are [%lu] remaining waypoints. Please " + "report this situation to the maintainers of RMF.", + new_mutex_group.c_str(), + context->requester_id().c_str(), + waypoints.size()); + } + } + + std::size_t first_excluded_route = 0; + std::stringstream ss; + if (current_mutex_group.has_value()) + { + ss << "truncating for switch from [" + << current_mutex_group->mutex_group + << "] to [" << new_mutex_group << "] for [" + << context->requester_id() << "]"; + } + else + { + ss << "truncating to lock [" << new_mutex_group << "] for [" + << context->requester_id() << "]"; + } + 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(); + ss << "\n -- erasing from checkpoint " << c.checkpoint_id + << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; + + ss << " t.size() before: " << t.size(); + t.erase(t.begin() + (int)c.checkpoint_id, t.end()); + ss << " vs after: " << t.size(); + + if (t.size() > 0) + { + ss << " ending at <" << t.back().position().transpose() << ">"; + } + } + std::cout << ss.str() << std::endl; + + 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()); + + auto next_itinerary = std::make_shared< + rmf_traffic::schedule::Itinerary>(full_itinerary); + auto data = LockMutexGroup::Data{ + new_mutex_group, + hold_map, + hold_position, + hold_time, + plan_id, + next_itinerary + }; + + previous_itinerary = data.resume_itinerary; + + return data; + }; + + const auto get_new_mutex_group = [&]( + const rmf_traffic::agv::Plan::Waypoint& wp) { std::string new_mutex_group; - if (it->graph_index().has_value()) + if (wp.graph_index().has_value()) { const auto& group = - graph.get_waypoint(*it->graph_index()).in_mutex_group(); + graph.get_waypoint(*wp.graph_index()).in_mutex_group(); if (!group.empty()) { std::cout << " === " << __LINE__ << ": " << group << std::endl; @@ -566,7 +707,7 @@ std::optional ExecutePlan::make( } } - for (const auto l : it->approach_lanes()) + for (const auto l : wp.approach_lanes()) { const auto& lane = graph.get_lane(l); const auto& group = lane.properties().in_mutex_group(); @@ -585,6 +726,17 @@ std::optional ExecutePlan::make( && current_mutex_group->mutex_group != new_mutex_group ); + return std::make_pair(mutex_group_change, new_mutex_group); + }; + + LegacyPhases legacy_phases; + while (!waypoints.empty()) + { + auto it = waypoints.begin(); + bool event_occurred = false; + for (; it != waypoints.end(); ++it) + { + const auto [mutex_group_change, new_mutex_group] = get_new_mutex_group(*it); std::cout << new_mutex_group << " | " << current_mutex_group.has_value(); if (current_mutex_group.has_value()) std::cout << " | " << current_mutex_group->mutex_group; @@ -595,107 +747,15 @@ std::optional ExecutePlan::make( std::cout << " === " << __LINE__ << ": " << new_mutex_group << std::endl; if (!new_mutex_group.empty()) { - const auto make_current_mutex_group = [&](const rmf_traffic::agv::Plan::Waypoint& wp) + if (move_through.size() > 1) { - 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 [%s] transition needed " - "by robot [%s]. There are [%lu] remaining waypoints. Please " - "report this situation to the maintainers of RMF.", - new_mutex_group.c_str(), - context->requester_id().c_str(), - waypoints.size()); - } - } + auto next_mutex_group = make_current_mutex_group( + new_mutex_group, move_through.back()); - std::size_t first_excluded_route = 0; - std::stringstream ss; if (current_mutex_group.has_value()) - { - ss << "truncating for switch from [" - << current_mutex_group->mutex_group - << "] to [" << new_mutex_group << "]"; - } + std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; else - { - ss << "truncating to lock [" << new_mutex_group << "]"; - } - 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(); - ss << "\n -- erasing up to checkpoint " << c.checkpoint_id << "\n"; - ss << "t.size() before: " << t.size(); - t.erase(t.begin() + (int)c.checkpoint_id, t.end()); - ss << " | after: " << t.size(); - } - std::cout << ss.str() << std::endl; - - 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()); - - auto next_itinerary = std::make_shared< - rmf_traffic::schedule::Itinerary>(full_itinerary); - auto data = LockMutexGroup::Data{ - new_mutex_group, - hold_map, - hold_position, - hold_time, - plan_id, - next_itinerary - }; - - previous_itinerary = data.resume_itinerary; - - return data; - }; - - if (move_through.size() > 1) - { - auto next_mutex_group = make_current_mutex_group(move_through.back()); - - std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; + std::cout << " === " << __LINE__ << ": " << std::endl; legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), @@ -722,14 +782,15 @@ std::optional ExecutePlan::make( // through yet. Just set the current_mutex_group from this point. if (move_through.empty()) { - current_mutex_group = make_current_mutex_group(*it); + current_mutex_group = make_current_mutex_group( + new_mutex_group, *it); std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; } else { assert(move_through.size() == 1); current_mutex_group = make_current_mutex_group( - move_through.back()); + new_mutex_group, move_through.back()); std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; } } @@ -744,7 +805,6 @@ std::optional ExecutePlan::make( if (it->event()) { - std::optional event_mutex_group; if (move_through.size() > 1) { legacy_phases.emplace_back( @@ -752,16 +812,20 @@ std::optional ExecutePlan::make( context, move_through, plan_id, tail_period), it->time(), it->dependencies(), current_mutex_group); } - else if(mutex_group_change) + + std::optional next_waypoint; + auto next_it = it + 1; + if (next_it != waypoints.end()) { - std::cout << " >>>> appending mutex group to event" << std::endl; - event_mutex_group = current_mutex_group; + next_waypoint = *next_it; } move_through.clear(); bool continuous = true; EventPhaseFactory factory( - context, legacy_phases, *it, plan_id, event_mutex_group, continuous); + context, legacy_phases, *it, next_waypoint, plan_id, + current_mutex_group, make_current_mutex_group, get_new_mutex_group, + continuous); it->event()->execute(factory); while (factory.moving_lift()) { @@ -785,7 +849,7 @@ std::optional ExecutePlan::make( "navigation graph. Please report this to the system integrator."); } - factory.waypoint = *it; + factory.initial_waypoint = *it; it->event()->execute(factory); } From c425b8793512ca1785c8ff678a1d020c14f8c281 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 21 Nov 2023 15:06:04 +0000 Subject: [PATCH 61/89] Iterating on implementation strategy Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 70 +++++++- .../rmf_fleet_adapter/agv/RobotContext.cpp | 52 +++++- .../rmf_fleet_adapter/agv/RobotContext.hpp | 15 +- .../agv/internal_RobotUpdateHandle.hpp | 9 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 3 + .../rmf_fleet_adapter/events/GoToPlace.cpp | 12 ++ .../events/LockMutexGroup.cpp | 164 ++++++++++++------ .../rmf_fleet_adapter/phases/DockRobot.cpp | 12 +- .../rmf_fleet_adapter/phases/DockRobot.hpp | 89 +++++++--- .../rmf_fleet_adapter/phases/MoveRobot.cpp | 14 +- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 74 ++++++-- 11 files changed, 392 insertions(+), 122 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 e142bc729..a8315fcb7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -773,8 +773,10 @@ struct ProgressTracker : std::enable_shared_from_this void next() { + std::cout << " >>> " << __LINE__ << " rq: " << reverse_queue.size() << std::endl; if (reverse_queue.empty()) { + std::cout << " >>> " << __LINE__ << " trigger finish" << std::endl; current_identifier = nullptr; finished.trigger(); return; @@ -786,16 +788,23 @@ struct ProgressTracker : std::enable_shared_from_this ::get(current_activity).identifier; auto& current_activity_impl = EasyFullControl::CommandExecution::Implementation::get(current_activity); + std::cout << " >>> " << __LINE__ << " make next finisher" << std::endl; current_activity_impl.finisher = [w_progress = weak_from_this()]() { + std::cout << " >>> " << __LINE__ << " lock progress" << std::endl; if (const auto progress = w_progress.lock()) { + std::cout << " >>> " << __LINE__ << " trigger next" << std::endl; progress->next(); } }; + + + std::cout << " >>> " << __LINE__ << " prepare begin" << std::endl; const auto begin = current_activity_impl.begin; if (begin) { + std::cout << " >>> " << __LINE__ << " do begin" << std::endl; begin(std::move(current_activity)); } } @@ -1357,9 +1366,26 @@ void EasyCommandHandle::follow_new_path( i1 = i0 + 1; } + auto finisher = [path_finished_callback_]() + { + std::cout << "CALLING THE PATH FINISHER" << std::endl; + if (path_finished_callback_) + { + path_finished_callback_(); + std::cout << "DONE CALLING THE PATH FINISHER" << std::endl; + } + else + { + std::cout << " ??? PATH FINISHED CALLBACK IS NULL" << std::endl; + } + }; + + std::cout << __LINE__ << ": SETTING CURRENT_PROGRESS FOR " << context->requester_id() + << " | " << waypoints.back().position().transpose() << std::endl; this->current_progress = ProgressTracker::make( queue, - path_finished_callback_); + // path_finished_callback_); + finisher); this->current_progress->next(); } @@ -1492,11 +1518,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)); + }); } }; @@ -1531,9 +1569,27 @@ void EasyCommandHandle::dock( { handle_nav_request(destination, execution); }); + + auto finisher = [docking_finished_callback_]() + { + std::cout << " CALLING THE DOCKING FINISHER" << std::endl; + if (docking_finished_callback_) + { + docking_finished_callback_(); + std::cout << " DONE CALLING THE DOCKING FINISHER" << std::endl; + } + else + { + std::cout << " ??? DOCKING FINISHED CALLBACK IS NULL" << std::endl; + } + }; + + std::cout << __LINE__ << ": SETTING CURRENT_PROGRESS FOR " << context->requester_id() + << " | " << command_position.transpose() << std::endl; this->current_progress = ProgressTracker::make( {std::move(cmd)}, - std::move(docking_finished_callback_)); + // std::move(docking_finished_callback_)); + finisher); this->current_progress->next(); } 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 86326df92..5340e784e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1038,19 +1038,23 @@ const std::string& RobotContext::locked_mutex_group() const } //============================================================================== -void RobotContext::request_mutex_group( +const rxcpp::observable& RobotContext::request_mutex_group( std::string group, rmf_traffic::Time claim_time) { if (group.empty()) { + RCLCPP_ERROR( + node()->get_logger(), + "Releasing mutexes for [%s] after a request to lock an empty mutex", + requester_id().c_str()); release_mutex_group(); - return; + return _mutex_group_switch_obs; } if (group == _requesting_mutex_group.name) { - return; + return _mutex_group_switch_obs; } _requesting_mutex_group = MutexGroupData{ @@ -1059,12 +1063,15 @@ void RobotContext::request_mutex_group( }; _publish_mutex_group_request(); + return _mutex_group_switch_obs; } //============================================================================== void RobotContext::release_mutex_group() { _release_mutex_group(_requesting_mutex_group); + std::cout << " === " << __LINE__ << " | releasing locked mutex [" << _locked_mutex_group.name + << "] for robot [" << requester_id() << "]" << std::endl; _release_mutex_group(_locked_mutex_group); } @@ -1110,8 +1117,8 @@ 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_switch_obs = _mutex_group_switch.get_observable(); _current_mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE; _override_status = std::nullopt; @@ -1247,13 +1254,17 @@ void RobotContext::_check_mutex_groups( { // Make sure to release any mutex groups that this robot is not trying to // lock right now. + std::cout << " ::: checking mutex groups for " << requester_id() << std::endl; for (const auto& assignment : states.assignments) { + std::cout << " ::: " << assignment.claimant << " != " << participant_id() << std::endl; if (assignment.claimant != participant_id()) - return; + continue; + std::cout << " ::: " << __LINE__ << " | " << assignment.group << " == " << _requesting_mutex_group.name << std::endl; if (assignment.group == _requesting_mutex_group.name) { + std::cout << " ::: " << __LINE__ << " | " << _locked_mutex_group.name << " != " << assignment.group << std::endl; if (_locked_mutex_group.name != assignment.group) { _node->mutex_group_request()->publish( @@ -1264,7 +1275,10 @@ void RobotContext::_check_mutex_groups( .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); } - _locked_mutex_group = _requesting_mutex_group; + std::cout << " ::: offering switch from [" << _locked_mutex_group.name + << "] to [" << _requesting_mutex_group.name << "] for [" << requester_id() + << "]" << std::endl; + _mutex_group_switch.get_subscriber().on_next(_make_mutex_group_switch()); } else if (assignment.group != _locked_mutex_group.name) { @@ -1280,6 +1294,32 @@ void RobotContext::_check_mutex_groups( } } +//============================================================================== +MutexGroupSwitch RobotContext::_make_mutex_group_switch() +{ + return MutexGroupSwitch{ + _locked_mutex_group.name, + _requesting_mutex_group.name, + [w = weak_from_this(), request = _requesting_mutex_group]() + { + const auto self = w.lock(); + if (!self) + return false; + + if (self->_requesting_mutex_group.name == request.name + && self->_requesting_mutex_group.claim_time == request.claim_time) + { + std::cout << " ::: " << __LINE__ << " | Setting locked mutex for " + << self->requester_id() << ": " << self->_locked_mutex_group.name << std::endl; + self->_locked_mutex_group = request; + return true; + } + + return false; + } + }; +} + //============================================================================== void RobotContext::_release_mutex_group(MutexGroupData& data) { 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 c722e657a..39dffffe0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -336,6 +336,14 @@ struct MutexGroupData builtin_interfaces::msg::Time claim_time; }; +//============================================================================== +struct MutexGroupSwitch +{ + std::string from; + std::string to; + std::function accept; +}; + //============================================================================== class RobotContext : public std::enable_shared_from_this, @@ -613,8 +621,8 @@ class RobotContext /// What mutex group is currently being locked. const std::string& locked_mutex_group() const; - /// Set the mutex group that this robot needs to have locked. - void request_mutex_group( + /// Set the mutex group that this robot needs to lock. + const rxcpp::observable& request_mutex_group( std::string group, rmf_traffic::Time claim_time); @@ -765,10 +773,13 @@ class RobotContext std::optional _initial_time_idle_outside_lift; void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); + MutexGroupSwitch _make_mutex_group_switch(); void _release_mutex_group(MutexGroupData& data); void _publish_mutex_group_request(); MutexGroupData _requesting_mutex_group; MutexGroupData _locked_mutex_group; + rxcpp::subjects::subject _mutex_group_switch; + rxcpp::observable _mutex_group_switch_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; 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 02baca36f..40b364f96 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 @@ -50,8 +50,10 @@ class TriggerOnce void trigger() const { + std::cout << " !!! " << __LINE__ << " triggering" << std::endl; if (!_callback) { + std::cout << " !!! " << __LINE__ << " no callback given" << std::endl; return; } @@ -60,9 +62,16 @@ class TriggerOnce std::shared_ptr> inner = *_callback; if (inner) { + std::cout << " !!! " << __LINE__ << " have inner" << std::endl; if (*inner) { + std::cout << " !!! " << __LINE__ << " triggering inner" << std::endl; (*inner)(); + std::cout << " !!! " << __LINE__ << " done triggering inner" << std::endl; + } + else + { + std::cout << " !!! " << __LINE__ << " inner was empty" << std::endl; } *inner = nullptr; 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 f2f0f229d..d8aed5b6b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -153,6 +153,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor } else { + std::cout << " === " << __LINE__ << " releasing mutex group after docking" << std::endl; _current_mutex_group = std::nullopt; } } @@ -797,6 +798,8 @@ std::optional ExecutePlan::make( } else { + std::cout << " === " << __LINE__ << " releasing mutex group at waypoint " + << it->position().transpose() << ">" << std::endl; current_mutex_group = std::nullopt; } } 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 6193b3753..c274401c0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -547,6 +547,18 @@ 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(); + if (graph.get_waypoint(_goal.waypoint()).in_mutex_group().empty()) + { + _context->release_mutex_group(); + } + _finished(); return; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index e9af5dce0..fb4ac5333 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -108,7 +108,9 @@ auto LockMutexGroup::Active::interrupt( void LockMutexGroup::Active::cancel() { _state->update_status(Status::Canceled); - _context->worker().schedule([finished = _finished](const auto&) + const auto finished = _finished; + _finished = nullptr; + _context->worker().schedule([finished](const auto&) { finished(); }); @@ -127,6 +129,9 @@ void LockMutexGroup::Active::_initialize() using MutexGroupStatesPtr = std::shared_ptr; + std::cout << "Currently locked group for [" << _context->requester_id() << "]: " + << _context->locked_mutex_group() + << " vs " << _data.mutex_group << std::endl; if (_context->locked_mutex_group() == _data.mutex_group) { RCLCPP_INFO( @@ -139,13 +144,23 @@ void LockMutexGroup::Active::_initialize() // We don't need to do anything further, we already got the mutex group // previously. _context->worker().schedule( - [finished = _finished](const auto&) + [state = _state, finished = _finished](const auto&) { + state->update_status(State::Status::Completed); finished(); }); return; } + *_data.plan_id += 1; + rmf_traffic::Trajectory hold; + const auto zero = Eigen::Vector3d::Zero(); + const auto wait = std::chrono::seconds(5); + hold.insert(_data.hold_time, _data.hold_position, zero); + hold.insert(_data.hold_time + wait, _data.hold_position, zero); + _stubborn = _context->be_stubborn(); + _schedule({rmf_traffic::Route(_data.hold_map, std::move(hold))}); + _state->update_log().info( "Waiting to lock mutex group [" + _data.mutex_group + "]"); RCLCPP_INFO( @@ -154,50 +169,47 @@ void LockMutexGroup::Active::_initialize() _data.mutex_group.c_str(), _context->requester_id().c_str()); - const auto cumulative_delay = _context->now() - _data.hold_time - - std::chrono::seconds(2); + const auto cumulative_delay = _context->now() - _data.hold_time; _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); - _listener = _context->node()->mutex_group_states() - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [weak = weak_from_this()]( - const MutexGroupStatesPtr& mutex_group_states) - { - const auto self = weak.lock(); - if (!self) - return; + // _listener = _context->node()->mutex_group_states() + // .observe_on(rxcpp::identity_same_worker(_context->worker())) + // .subscribe( + // [weak = weak_from_this()]( + // const MutexGroupStatesPtr& mutex_group_states) + // { + // const auto self = weak.lock(); + // if (!self) + // return; - for (const auto& assignment : mutex_group_states->assignments) - { - if (assignment.group == self->_data.mutex_group) - { - if (assignment.claimant == self->_context->participant_id()) - { - const auto finished = self->_finished; - self->_finished = nullptr; - if (finished) - { - if (!self->_data.resume_itinerary->empty()) - { - self->_schedule(*self->_data.resume_itinerary); - } - std::cout << " === LOCKED MUTEX " << __LINE__ << " new plan id for " - << self->_context->requester_id() << ": " - << *self->_data.plan_id << std::endl; - self->_state->update_status(State::Status::Completed); - RCLCPP_INFO( - self->_context->node()->get_logger(), - "Finished locking mutex group [%s] for robot [%s]", - self->_data.mutex_group.c_str(), - self->_context->requester_id().c_str()); - finished(); - return; - } - } - } - } - }); + // for (const auto& assignment : mutex_group_states->assignments) + // { + // if (assignment.group == self->_data.mutex_group) + // { + // if (assignment.claimant == self->_context->participant_id()) + // { + // const auto finished = self->_finished; + // self->_finished = nullptr; + // if (finished) + // { + // std::cout << " === LOCKED MUTEX " << __LINE__ << std::endl; + // self->_state->update_status(State::Status::Completed); + // RCLCPP_INFO( + // self->_context->node()->get_logger(), + // "Requesting replan for robot [%s] after locking mutex group " + // "[%s]", + // self->_context->requester_id().c_str(), + // self->_data.mutex_group.c_str()); + + // // Things may have changed while waiting for the mutex to be + // // locked, so we should replan. + // self->_context->request_replan(); + // return; + // } + // } + // } + // } + // }); _delay_timer = _context->node()->try_create_wall_timer( std::chrono::seconds(1), @@ -214,7 +226,50 @@ void LockMutexGroup::Active::_initialize() std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() << " TO " << _data.mutex_group << std::endl; - _context->request_mutex_group(_data.mutex_group, _data.hold_time); + _listener = _context->request_mutex_group(_data.mutex_group, _data.hold_time) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe([w = weak_from_this()](const agv::MutexGroupSwitch& consider) + { + const auto self = w.lock(); + if (!self) + return; + + if (consider.to == self->_data.mutex_group) + { + const auto now = self->_context->now(); + if (now - self->_data.hold_time > std::chrono::seconds(2)) + { + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Replanning for [%s] after a delay in locking mutex [%s]", + self->_context->requester_id().c_str(), + self->_data.mutex_group.c_str()); + self->_context->request_replan(); + self->_finished = nullptr; + return; + } + + // We locked the mutex quickly enough that we should proceed. + if (consider.accept()) + { + const auto finished = self->_finished; + self->_finished = nullptr; + if (finished) + { + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Finished locking mutex [%s] for [%s]", + self->_data.mutex_group.c_str(), + self->_context->requester_id().c_str()); + + self->_schedule(*self->_data.resume_itinerary); + self->_state->update_status(Status::Completed); + finished(); + return; + } + } + } + }); } //============================================================================== @@ -242,19 +297,22 @@ void LockMutexGroup::Active::_schedule( break; } - *_data.plan_id = _context->itinerary().assign_plan_id(); scheduled = _context->itinerary().set(*_data.plan_id, itinerary); if (!scheduled) { - RCLCPP_ERROR( - _context->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.", - *_data.plan_id, - _context->itinerary().current_plan_id(), - _context->requester_id().c_str()); + *_data.plan_id = _context->itinerary().assign_plan_id(); + if (attempts > 1) + { + RCLCPP_ERROR( + _context->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.", + *_data.plan_id, + _context->itinerary().current_plan_id(), + _context->requester_id().c_str()); + } } } } 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 1ff2a9628..a5df28611 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp @@ -36,9 +36,6 @@ DockRobot::ActivePhase::ActivePhase( 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); } @@ -46,7 +43,7 @@ DockRobot::ActivePhase::ActivePhase( const rxcpp::observable& DockRobot::ActivePhase::observe() const { - return _obs; + return obs; } //============================================================================== @@ -106,8 +103,11 @@ std::shared_ptr DockRobot::PendingPhase::begin() "critical internal error, please report this bug to the RMF maintainers.", _context->requester_id().c_str()); } - return std::make_shared( + 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; } //============================================================================== @@ -124,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 647cc5b39..5ce3d1e4b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -29,7 +29,8 @@ struct DockRobot { class Action; - class ActivePhase : public LegacyTask::ActivePhase + class ActivePhase : public LegacyTask::ActivePhase, + public std::enable_shared_from_this { public: @@ -49,6 +50,8 @@ struct DockRobot const std::string& description() const override; + std::shared_ptr action; + rxcpp::observable obs; private: friend class Action; @@ -57,8 +60,6 @@ struct DockRobot std::string _description; rmf_traffic::agv::Plan::Waypoint _waypoint; rmf_traffic::PlanId _plan_id; - std::shared_ptr _action; - rxcpp::observable _obs; std::shared_ptr _be_stubborn; }; @@ -91,13 +92,13 @@ struct DockRobot { public: - Action(ActivePhase* phase); + Action(std::weak_ptr phase); template void operator()(const Subscriber& s); private: - ActivePhase* _phase; + std::weak_ptr _phase; }; }; @@ -105,33 +106,65 @@ 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, - wp = _phase->_waypoint, plan_id = _phase->_plan_id]() + 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; - for (const auto& c : wp.arrival_checkpoints()) - { - std::cout << "docking reached " << context->participant_id() - << " | " << context->itinerary().current_plan_id() - << ":" << c.route_id << ":" << c.checkpoint_id - << " #" << context->itinerary().progress_version() << std::endl; - context->itinerary().reached(plan_id, c.route_id, c.checkpoint_id); - } + 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]() + { + std::cout << "Docking finish callback triggered" << std::endl; + context->worker().schedule( + [s, dock_name, context, wp, plan_id](const auto&) + { + std::cout << "Finished docking [" + context->requester_id() + << "] into dock [" << dock_name << "]" << std::endl; + 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()) + { + std::cout << "docking reached " << context->participant_id() + << " | " << context->itinerary().current_plan_id() + << ":" << c.route_id << ":" << c.checkpoint_id + << " #" << context->itinerary().progress_version() << std::endl; + context->itinerary().reached(plan_id, c.route_id, c.checkpoint_id); + } + + if (wp.graph_index().has_value()) + { + const auto& graph = context->navigation_graph(); + if (graph.get_waypoint(*wp.graph_index()).in_mutex_group().empty()) + { + std::cout << "Releasing mutex group [" + << context->locked_mutex_group() << "] for [" + << context->requester_id() << "] after docking finished" + << std::endl; + context->release_mutex_group(); + } + } + + s.on_next(status); + s.on_completed(); + }); + }); }); } 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 c8d7c032d..aeabf6ea0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -93,7 +93,8 @@ 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(); } @@ -141,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 bebc51a54..ad684ecb9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -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; @@ -167,9 +168,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) self->_context->request_replan(); }); - _context->command()->follow_new_path( - _waypoints, - [s, w_action = weak_from_this(), r = _context->requester_id()]( + const auto update = [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(); @@ -289,17 +288,8 @@ void MoveRobot::Action::operator()(const Subscriber& s) { const auto adjusted_now = now - new_cumulative_delay; const auto& graph = context->navigation_graph(); - std::size_t count = 0; for (const auto& wp : self->_waypoints) { - if (count == 0) - { - // The first waypoint doesn't always have a mutex group - // associated. - ++count; - continue; - } - if (wp.time() > adjusted_now) { break; @@ -307,12 +297,29 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (wp.graph_index().has_value()) { + if (self->_first_graph_index.has_value()) + { + const auto nav = self->_context->nav_params(); + const auto i_wp = *wp.graph_index(); + const auto i_first = *self->_first_graph_index; + bool ignore = + (nav && nav->in_same_stack(i_wp, i_first)) + || i_wp == i_first; + + if (ignore) + { + // The first waypoint doesn't always have a mutex group + // associated. + continue; + } + } + const auto& g = graph.get_waypoint(*wp.graph_index()) .in_mutex_group(); if (g.empty()) { - std::cout << __LINE__ << ": Releasing mutex for " << *wp.graph_index() - << " index " << count + std::cout << __LINE__ << ": Releasing mutex at " << *wp.graph_index() + << " <" << wp.position().transpose() << ">" << " | " << rmf_traffic::time::to_seconds(wp.time().time_since_epoch()) << " vs " << rmf_traffic::time::to_seconds(adjusted_now.time_since_epoch()) << std::endl; @@ -320,14 +327,14 @@ void MoveRobot::Action::operator()(const Subscriber& s) break; } } - - ++count; } } }); - }, - [s, w = weak_from_this()]() + }; + + const auto finish = [s, w = weak_from_this(), name = _context->requester_id()]() { + std::cout << "PATH FINISHER TRIGGERED" << std::endl; if (const auto self = w.lock()) { if (!self->_waypoints.empty()) @@ -352,8 +359,17 @@ void MoveRobot::Action::operator()(const Subscriber& s) std::cout << __LINE__ << ": Releasing mutex at end of path" << std::endl; self->_context->release_mutex_group(); } + else + { + std::cout << __LINE__ << ": Not releasing because last waypoint still has mutex: " + << graph.get_waypoint(*last_index).in_mutex_group() << std::endl; + } } } + else + { + std::cout << __LINE__ << " EMPTY WAYPOINTS??? FOR " << self->_context->requester_id() << std::endl; + } LegacyTask::StatusMsg msg; msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; @@ -361,6 +377,28 @@ void MoveRobot::Action::operator()(const Subscriber& s) s.on_next(msg); s.on_completed(); } + else + { + std::cout << " ###### MOVE ROBOT IS PREMATURELY DEAD FOR " << name << std::endl; + } + }; + + _context->command()->follow_new_path( + _waypoints, + [worker = _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 = _context->worker(), finish]() + { + worker.schedule([finish](const auto&) + { + finish(); + }); }); } From 9d6f55338fd077d21c62450f913da4830cf587a7 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 22 Nov 2023 15:47:25 +0000 Subject: [PATCH 62/89] Support multiple simultaneous mutex group locks Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 179 +++++------- .../rmf_fleet_adapter/agv/RobotContext.hpp | 35 +-- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 264 ++++++++++-------- .../rmf_fleet_adapter/events/GoToPlace.cpp | 9 +- .../events/LockMutexGroup.cpp | 126 ++++----- .../events/LockMutexGroup.hpp | 8 +- .../events/WaitForTraffic.cpp | 2 +- .../rmf_fleet_adapter/phases/DockRobot.hpp | 10 +- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 67 ++--- 9 files changed, 328 insertions(+), 372 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 5340e784e..c07205501 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -369,51 +369,51 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) _location = std::move(location_); filter_closed_lanes(); - // const auto& graph = navigation_graph(); + // std::stringstream ss; + // ss << requester_id() << " locations"; // for (const auto& l : _location) // { - // std::cout << " -- "; + // ss << " | "; // if (l.lane().has_value()) // { - // std::cout << "lane[" << *l.lane() << "] "; - // Printer printer; + // ss << "lane[" << *l.lane() << "] "; + // EventPrinter printer; // const auto& lane = graph.get_lane(*l.lane()); // if (lane.entry().event()) // { - // std::cout << " [entry "; + // ss << " [entry "; // lane.entry().event()->execute(printer); - // std::cout << "] "; + // ss << "] "; // } // const auto& i_wp0 = lane.entry().waypoint_index(); // const auto& wp0 = graph.get_waypoint(i_wp0); // const auto& i_wp1 = lane.exit().waypoint_index(); // const auto& wp1 = graph.get_waypoint(i_wp1); - // std::cout << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; + // ss << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; // if (const auto lift = wp0.in_lift()) - // std::cout << "{" << lift->name() << "} "; - // std::cout << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; + // ss << "{" << lift->name() << "} "; + // ss << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; // if (const auto lift = wp1.in_lift()) - // std::cout << "{" << lift->name() << "} "; + // ss << "{" << lift->name() << "} "; // if (lane.exit().event()) // { - // std::cout << "[exit "; + // ss << "[exit "; // lane.exit().event()->execute(printer); - // std::cout << "]"; + // ss << "]"; // } - // std::cout << " | "; + // ss << " | "; // } - // std::cout << l.waypoint(); + // ss << l.waypoint(); // const auto& wp = graph.get_waypoint(l.waypoint()); // if (const auto lift = wp.in_lift()) // { - // std::cout << " {" << lift->name() << "}"; + // ss << " {" << lift->name() << "}"; // } // if (l.location().has_value()) // { - // std::cout << " | " << l.location()->transpose(); + // ss << " | " << l.location()->transpose(); // } - // std::cout << std::endl; // } if (_location.empty()) @@ -1032,47 +1032,38 @@ void RobotContext::release_lift() } //============================================================================== -const std::string& RobotContext::locked_mutex_group() const +const std::unordered_map& +RobotContext::locked_mutex_groups() const { - return _locked_mutex_group.name; + return _locked_mutex_groups; } //============================================================================== -const rxcpp::observable& RobotContext::request_mutex_group( - std::string group, +const rxcpp::observable& RobotContext::request_mutex_groups( + std::unordered_set groups, rmf_traffic::Time claim_time) { - if (group.empty()) + const auto t = rmf_traffic_ros2::convert(claim_time); + for (const auto& group : groups) { - RCLCPP_ERROR( - node()->get_logger(), - "Releasing mutexes for [%s] after a request to lock an empty mutex", - requester_id().c_str()); - release_mutex_group(); - return _mutex_group_switch_obs; - } - - if (group == _requesting_mutex_group.name) - { - return _mutex_group_switch_obs; + const auto [it, inserted] = _requesting_mutex_groups.insert({group, t}); + if (!inserted) + { + if (t.nanosec < it->second.nanosec) + it->second = t; + } } - _requesting_mutex_group = MutexGroupData{ - std::move(group), - rmf_traffic_ros2::convert(claim_time) - }; - - _publish_mutex_group_request(); - return _mutex_group_switch_obs; + _publish_mutex_group_requests(); + return _mutex_group_lock_obs; } //============================================================================== -void RobotContext::release_mutex_group() +void RobotContext::retain_mutex_groups( + const std::unordered_set& retain) { - _release_mutex_group(_requesting_mutex_group); - std::cout << " === " << __LINE__ << " | releasing locked mutex [" << _locked_mutex_group.name - << "] for robot [" << requester_id() << "]" << std::endl; - _release_mutex_group(_locked_mutex_group); + _retain_mutex_groups(retain, _requesting_mutex_groups); + _retain_mutex_groups(retain, _locked_mutex_groups); } //============================================================================== @@ -1118,7 +1109,7 @@ RobotContext::RobotContext( _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_switch_obs = _mutex_group_switch.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; @@ -1254,36 +1245,21 @@ void RobotContext::_check_mutex_groups( { // Make sure to release any mutex groups that this robot is not trying to // lock right now. - std::cout << " ::: checking mutex groups for " << requester_id() << std::endl; for (const auto& assignment : states.assignments) { - std::cout << " ::: " << assignment.claimant << " != " << participant_id() << std::endl; if (assignment.claimant != participant_id()) continue; - std::cout << " ::: " << __LINE__ << " | " << assignment.group << " == " << _requesting_mutex_group.name << std::endl; - if (assignment.group == _requesting_mutex_group.name) + if (_requesting_mutex_groups.count(assignment.group) > 0) { - std::cout << " ::: " << __LINE__ << " | " << _locked_mutex_group.name << " != " << assignment.group << std::endl; - if (_locked_mutex_group.name != assignment.group) - { - _node->mutex_group_request()->publish( - rmf_fleet_msgs::build() - .group(_locked_mutex_group.name) - .claimant(participant_id()) - .claim_time(_locked_mutex_group.claim_time) - .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); - } - - std::cout << " ::: offering switch from [" << _locked_mutex_group.name - << "] to [" << _requesting_mutex_group.name << "] for [" << requester_id() - << "]" << std::endl; - _mutex_group_switch.get_subscriber().on_next(_make_mutex_group_switch()); + _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 (assignment.group != _locked_mutex_group.name) + else if (_locked_mutex_groups.count(assignment.group) == 0) { - // This assignment does not match either the requested nor the currently - // locked mutex group. This is an error so let's release it. + // 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) @@ -1294,34 +1270,28 @@ void RobotContext::_check_mutex_groups( } } -//============================================================================== -MutexGroupSwitch RobotContext::_make_mutex_group_switch() +void RobotContext::_retain_mutex_groups( + const std::unordered_set& retain, + std::unordered_map& groups) { - return MutexGroupSwitch{ - _locked_mutex_group.name, - _requesting_mutex_group.name, - [w = weak_from_this(), request = _requesting_mutex_group]() + std::vector release; + for (const auto& [name, time] : groups) + { + if (retain.count(name) == 0) { - const auto self = w.lock(); - if (!self) - return false; - - if (self->_requesting_mutex_group.name == request.name - && self->_requesting_mutex_group.claim_time == request.claim_time) - { - std::cout << " ::: " << __LINE__ << " | Setting locked mutex for " - << self->requester_id() << ": " << self->_locked_mutex_group.name << std::endl; - self->_locked_mutex_group = request; - return true; - } - - return false; + 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(MutexGroupData& data) +void RobotContext::_release_mutex_group(const MutexGroupData& data) const { if (data.name.empty()) { @@ -1334,11 +1304,10 @@ void RobotContext::_release_mutex_group(MutexGroupData& data) .claimant(participant_id()) .claim_time(data.claim_time) .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); - data.name = ""; } //============================================================================== -void RobotContext::_publish_mutex_group_request() +void RobotContext::_publish_mutex_group_requests() { const auto now = std::chrono::steady_clock::now(); if (_current_task_id.has_value()) @@ -1351,8 +1320,8 @@ void RobotContext::_publish_mutex_group_request() { // The robot has been idle for 10 seconds. It should not be keeping a // mutex locked; a task probably ended wrongly. - if (!_requesting_mutex_group.name.empty() - || !_locked_mutex_group.name.empty()) + if (!_requesting_mutex_groups.empty() + || !_locked_mutex_groups.empty()) { auto warning = [&](const std::string& name) { @@ -1364,17 +1333,19 @@ void RobotContext::_publish_mutex_group_request() requester_id().c_str()); }; - if (!_requesting_mutex_group.name.empty()) + for (const auto& [name, time] : _requesting_mutex_groups) { - warning(_requesting_mutex_group.name); + warning(name); + _release_mutex_group(MutexGroupData{name, time}); } + _requesting_mutex_groups.clear(); - if (!_locked_mutex_group.name.empty()) + for (const auto& [name, time] : _locked_mutex_groups) { - warning(_locked_mutex_group.name); + warning(name); + _release_mutex_group(MutexGroupData{name, time}); } - - release_mutex_group(); + _locked_mutex_groups.clear(); } } } @@ -1389,14 +1360,14 @@ void RobotContext::_publish_mutex_group_request() .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); }; - if (!_requesting_mutex_group.name.empty()) + for (const auto& [name, time] : _requesting_mutex_groups) { - publish(_requesting_mutex_group); + publish(MutexGroupData{name, time}); } - if (!_locked_mutex_group.name.empty()) + for (const auto& [name, time] : _locked_mutex_groups) { - publish(_locked_mutex_group); + publish(MutexGroupData{name, time}); } } 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 39dffffe0..d4d0d0fcc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -59,6 +59,7 @@ 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 @@ -191,11 +192,11 @@ inline std::string print_starts( if (l.location().has_value()) { - ss << " | exact location <" << l.location()->transpose() << ">"; + ss << " | location <" << l.location()->transpose() << ">"; } else { - ss << " | exactly on waypoint"; + ss << " | on waypoint"; } ss << " | orientation " << l.orientation() * 180.0 / M_PI; @@ -333,7 +334,7 @@ struct LiftDestination struct MutexGroupData { std::string name; - builtin_interfaces::msg::Time claim_time; + TimeMsg claim_time; }; //============================================================================== @@ -619,15 +620,15 @@ class RobotContext void release_lift(); /// What mutex group is currently being locked. - const std::string& locked_mutex_group() const; + const std::unordered_map& locked_mutex_groups() const; /// Set the mutex group that this robot needs to lock. - const rxcpp::observable& request_mutex_group( - std::string group, + const rxcpp::observable& request_mutex_groups( + std::unordered_set groups, rmf_traffic::Time claim_time); - /// If we are holding a mutex group, release it - void release_mutex_group(); + /// Retain only the mutex groups listed in the set. Release all others. + void retain_mutex_groups(const std::unordered_set& groups); /// Set the task manager for this robot. This should only be called in the /// TaskManager::make function. @@ -681,7 +682,7 @@ class RobotContext if (!self) return; - self->_publish_mutex_group_request(); + self->_publish_mutex_group_requests(); }); return context; @@ -773,13 +774,15 @@ class RobotContext std::optional _initial_time_idle_outside_lift; void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); - MutexGroupSwitch _make_mutex_group_switch(); - void _release_mutex_group(MutexGroupData& data); - void _publish_mutex_group_request(); - MutexGroupData _requesting_mutex_group; - MutexGroupData _locked_mutex_group; - rxcpp::subjects::subject _mutex_group_switch; - rxcpp::observable _mutex_group_switch_obs; + 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; 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 d8aed5b6b..6d054c471 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -113,10 +113,10 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor const PlanIdPtr plan_id, std::optional& current_mutex_group, std::function make_current_mutex_group, - std::function( - const rmf_traffic::agv::Plan::Waypoint&)> get_new_mutex_group, + const std::unordered_set&, + const rmf_traffic::agv::Plan::Waypoint&)> make_current_mutex_groups, + std::function>( + const rmf_traffic::agv::Plan::Waypoint&)> get_new_mutex_groups, bool& continuous) : initial_waypoint(initial_waypoint_), next_waypoint(std::move(next_waypoint_)), @@ -125,8 +125,8 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _event_start_time(initial_waypoint_.time()), _plan_id(plan_id), _current_mutex_group(current_mutex_group), - _make_current_mutex_group(std::move(make_current_mutex_group)), - _get_new_mutex_group(std::move(get_new_mutex_group)), + _make_current_mutex_groups(std::move(make_current_mutex_groups)), + _get_new_mutex_group(std::move(get_new_mutex_groups)), _continuous(continuous) { // Do nothing @@ -140,22 +140,14 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor std::optional event_mutex_group; if (next_waypoint.has_value() && next_waypoint->graph_index().has_value()) { - const auto [mutex_group_change, new_mutex_group] = + const auto [mutex_group_change, new_mutex_groups] = _get_new_mutex_group(*next_waypoint); if (mutex_group_change) { - if (!new_mutex_group.empty()) - { - event_mutex_group = _make_current_mutex_group( - new_mutex_group, initial_waypoint); - _current_mutex_group = event_mutex_group; - } - else - { - std::cout << " === " << __LINE__ << " releasing mutex group after docking" << std::endl; - _current_mutex_group = std::nullopt; - } + event_mutex_group = _make_current_mutex_groups( + new_mutex_groups, initial_waypoint); + _current_mutex_group = event_mutex_group; } } @@ -282,9 +274,9 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor PlanIdPtr _plan_id; std::optional& _current_mutex_group; std::function _make_current_mutex_group; - std::function( + const std::unordered_set&, + const rmf_traffic::agv::Plan::Waypoint&)> _make_current_mutex_groups; + std::function>( const rmf_traffic::agv::Plan::Waypoint&)> _get_new_mutex_group; bool& _continuous; bool _moving_lift = false; @@ -584,10 +576,10 @@ std::optional ExecutePlan::make( std::cout << pss.str() << std::endl; std::vector move_through; - std::optional current_mutex_group; + std::optional current_mutex_groups; - const auto make_current_mutex_group = [&]( - const std::string& new_mutex_group, + const auto make_current_mutex_groups = [&]( + const std::unordered_set& new_mutex_groups, const rmf_traffic::agv::Plan::Waypoint& wp) { const rmf_traffic::Time hold_time = wp.time(); @@ -615,47 +607,46 @@ std::optional ExecutePlan::make( { RCLCPP_ERROR( context->node()->get_logger(), - "Cannot find a map for a mutex group [%s] transition needed " + "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.", - new_mutex_group.c_str(), context->requester_id().c_str(), waypoints.size()); } } std::size_t first_excluded_route = 0; - std::stringstream ss; - if (current_mutex_group.has_value()) - { - ss << "truncating for switch from [" - << current_mutex_group->mutex_group - << "] to [" << new_mutex_group << "] for [" - << context->requester_id() << "]"; - } - else - { - ss << "truncating to lock [" << new_mutex_group << "] for [" - << context->requester_id() << "]"; - } + // std::stringstream ss; + // if (current_mutex_groups.has_value()) + // { + // ss << "truncating for switch from [" + // << current_mutex_groups->all_groups_str() + // << "] to [" << new_mutex_groups << "] for [" + // << context->requester_id() << "]"; + // } + // else + // { + // ss << "truncating to lock [" << new_mutex_groups << "] for [" + // << context->requester_id() << "]"; + // } 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(); - ss << "\n -- erasing from checkpoint " << c.checkpoint_id - << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; + // ss << "\n -- erasing from checkpoint " << c.checkpoint_id + // << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; - ss << " t.size() before: " << t.size(); + // ss << " t.size() before: " << t.size(); t.erase(t.begin() + (int)c.checkpoint_id, t.end()); - ss << " vs after: " << t.size(); + // ss << " vs after: " << t.size(); - if (t.size() > 0) - { - ss << " ending at <" << t.back().position().transpose() << ">"; - } + // if (t.size() > 0) + // { + // ss << " ending at <" << t.back().position().transpose() << ">"; + // } } - std::cout << ss.str() << std::endl; + // std::cout << ss.str() << std::endl; for (std::size_t i=0; i < previous_itinerary->size(); ++i) { @@ -680,7 +671,7 @@ std::optional ExecutePlan::make( auto next_itinerary = std::make_shared< rmf_traffic::schedule::Itinerary>(full_itinerary); auto data = LockMutexGroup::Data{ - new_mutex_group, + new_mutex_groups, hold_map, hold_position, hold_time, @@ -693,41 +684,78 @@ std::optional ExecutePlan::make( return data; }; - const auto get_new_mutex_group = [&]( + const auto get_new_mutex_groups = [&]( const rmf_traffic::agv::Plan::Waypoint& wp) { - std::string new_mutex_group; + std::stringstream ss; + std::unordered_set new_mutex_groups; + ss << "<" << wp.position().transpose() << "> "; if (wp.graph_index().has_value()) { + ss << "i_wp: " << *wp.graph_index() << " "; + if (const auto* name = graph.get_waypoint(*wp.graph_index()).name()) + { + ss << "name_wp: " << *name << " "; + } + const auto& group = graph.get_waypoint(*wp.graph_index()).in_mutex_group(); if (!group.empty()) { - std::cout << " === " << __LINE__ << ": " << group << std::endl; - new_mutex_group = group; + ss << " [" << group << "]"; + new_mutex_groups.insert(group); } } + ss << " lanes: "; for (const auto l : wp.approach_lanes()) { + ss << l << " "; const auto& lane = graph.get_lane(l); const auto& group = lane.properties().in_mutex_group(); if (!group.empty()) { - std::cout << " === " << __LINE__ << ": " << group << std::endl; - new_mutex_group = group; + ss << "[" << group << "] "; + new_mutex_groups.insert(group); break; } } - const bool mutex_group_change = - (!new_mutex_group.empty() && !current_mutex_group.has_value()) - || ( - current_mutex_group.has_value() - && current_mutex_group->mutex_group != new_mutex_group - ); + std::cout << " === " << __LINE__ << " " << all_str(new_mutex_groups) + << " | " << ss.str() << std::endl; + + bool mutex_group_change = + (!new_mutex_groups.empty() && !current_mutex_groups.has_value()); - return std::make_pair(mutex_group_change, new_mutex_group); + if (!mutex_group_change && current_mutex_groups.has_value()) + { + for (const auto& new_group : new_mutex_groups) + { + if (current_mutex_groups->mutex_groups.count(new_group) == 0) + { + mutex_group_change = true; + break; + } + } + } + + if (!mutex_group_change && current_mutex_groups.has_value()) + { + // We don't need to lock any new mutex groups, but we may be releasing + // some. We should reduce the current group set to whatever is in the + // new group set so that if we need to add more groups later then we are + // ready to do so. + if (new_mutex_groups.empty()) + { + current_mutex_groups = std::nullopt; + } + else + { + current_mutex_groups->mutex_groups = new_mutex_groups; + } + } + + return std::make_pair(mutex_group_change, new_mutex_groups); }; LegacyPhases legacy_phases; @@ -737,71 +765,61 @@ std::optional ExecutePlan::make( bool event_occurred = false; for (; it != waypoints.end(); ++it) { - const auto [mutex_group_change, new_mutex_group] = get_new_mutex_group(*it); - std::cout << new_mutex_group << " | " << current_mutex_group.has_value(); - if (current_mutex_group.has_value()) - std::cout << " | " << current_mutex_group->mutex_group; - std::cout << std::endl; + const auto [mutex_group_change, new_mutex_groups] = get_new_mutex_groups(*it); + // std::cout << new_mutex_groups << " | " << current_mutex_groups.has_value(); + // if (current_mutex_groups.has_value()) + // std::cout << " | " << current_mutex_groups->all_groups_str(); + // std::cout << std::endl; if (mutex_group_change) { - std::cout << " === " << __LINE__ << ": " << new_mutex_group << std::endl; - if (!new_mutex_group.empty()) + if (move_through.size() > 1) + { + auto next_mutex_group = make_current_mutex_groups( + new_mutex_groups, move_through.back()); + + // if (current_mutex_groups.has_value()) + // std::cout << " === " << __LINE__ << ": " << current_mutex_groups->mutex_group << std::endl; + // else + // std::cout << " === " << __LINE__ << ": " << std::endl; + 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 { - if (move_through.size() > 1) + // 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()) { - auto next_mutex_group = make_current_mutex_group( - new_mutex_group, move_through.back()); - - if (current_mutex_group.has_value()) - std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; - else - std::cout << " === " << __LINE__ << ": " << std::endl; - legacy_phases.emplace_back( - std::make_shared( - context, move_through, plan_id, tail_period), - move_through.back().time(), move_through.back().dependencies(), - current_mutex_group); - - 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_group = next_mutex_group; - - // We treat this the same as an event occurring to indicate that - // we should keep looping. - event_occurred = true; - break; + current_mutex_groups = make_current_mutex_groups( + new_mutex_groups, *it); + std::cout << " === " << __LINE__ << ": " << current_mutex_groups->all_groups_str() << std::endl; } else { - // We don't need to put in a break because nothing has been moved - // through yet. Just set the current_mutex_group from this point. - if (move_through.empty()) - { - current_mutex_group = make_current_mutex_group( - new_mutex_group, *it); - std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; - } - else - { - assert(move_through.size() == 1); - current_mutex_group = make_current_mutex_group( - new_mutex_group, move_through.back()); - std::cout << " === " << __LINE__ << ": " << current_mutex_group->mutex_group << std::endl; - } + assert(move_through.size() == 1); + current_mutex_groups = make_current_mutex_groups( + new_mutex_groups, move_through.back()); + std::cout << " === " << __LINE__ << ": " << current_mutex_groups->all_groups_str() << std::endl; } } - else - { - std::cout << " === " << __LINE__ << " releasing mutex group at waypoint " - << it->position().transpose() << ">" << std::endl; - current_mutex_group = std::nullopt; - } } move_through.push_back(*it); @@ -813,7 +831,7 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies(), current_mutex_group); + it->time(), it->dependencies(), current_mutex_groups); } std::optional next_waypoint; @@ -827,7 +845,7 @@ std::optional ExecutePlan::make( bool continuous = true; EventPhaseFactory factory( context, legacy_phases, *it, next_waypoint, plan_id, - current_mutex_group, make_current_mutex_group, get_new_mutex_group, + current_mutex_groups, make_current_mutex_groups, get_new_mutex_groups, continuous); it->event()->execute(factory); while (factory.moving_lift()) @@ -874,12 +892,12 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies(), current_mutex_group); + it->time(), it->dependencies(), current_mutex_groups); } else { legacy_phases.emplace_back( - nullptr, it->time(), it->dependencies(), current_mutex_group); + nullptr, it->time(), it->dependencies(), current_mutex_groups); } // Have the next sequence of waypoints begin with this one. @@ -904,7 +922,7 @@ std::optional ExecutePlan::make( std::make_shared( context, move_through, plan_id, tail_period), finish_time_estimate.value(), rmf_traffic::Dependencies{}, - current_mutex_group); + current_mutex_groups); } if (!event_occurred) @@ -943,7 +961,7 @@ std::optional ExecutePlan::make( { if (head->mutex_group_dependency.has_value()) { - ss << "\n -- Lock mutex group [" << head->mutex_group_dependency->mutex_group << "]"; + ss << "\n -- Lock mutex group [" << head->mutex_group_dependency->all_groups_str() << "] " << head->mutex_group_dependency->hold_position.transpose(); standbys.push_back(make_wait_for_mutex( context, event_id, head->mutex_group_dependency.value())); } 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 c274401c0..534e57f38 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -395,6 +395,9 @@ 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::cout << _context->requester_id() << " locations " << agv::print_starts(_context->location(), graph) << std::endl; + // TODO(MXG): Make the planning time limit configurable _find_path_service = std::make_shared( _context->planner(), _context->location(), _goal, @@ -554,10 +557,8 @@ void GoToPlace::Active::_execute_plan( _goal.waypoint()); const auto& graph = _context->navigation_graph(); - if (graph.get_waypoint(_goal.waypoint()).in_mutex_group().empty()) - { - _context->release_mutex_group(); - } + _context->retain_mutex_groups( + {graph.get_waypoint(_goal.waypoint()).in_mutex_group()}); _finished(); return; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index fb4ac5333..f06588100 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -20,6 +20,23 @@ 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, @@ -31,8 +48,8 @@ auto LockMutexGroup::Standby::make( standby->_context = std::move(context); standby->_state = rmf_task::events::SimpleEventState::make( id->assign(), - "Lock mutex group [" + data.mutex_group + "]", - "Waiting for the mutex group to be locked", + "Lock mutex groups " + data.all_groups_str(), + "Waiting for the mutex groups to be locked", rmf_task::Event::Status::Standby, {}, standby->_context->clock()); standby->_data = std::move(data); return standby; @@ -129,15 +146,17 @@ void LockMutexGroup::Active::_initialize() using MutexGroupStatesPtr = std::shared_ptr; - std::cout << "Currently locked group for [" << _context->requester_id() << "]: " - << _context->locked_mutex_group() - << " vs " << _data.mutex_group << std::endl; - if (_context->locked_mutex_group() == _data.mutex_group) + _remaining = _data.mutex_groups; + for (const auto& [locked, _] : _context->locked_mutex_groups()) + { + _remaining.erase(locked); + } + + if (_remaining.empty()) { RCLCPP_INFO( _context->node()->get_logger(), - "Mutex group [%s] was already locked for [%s]", - _data.mutex_group.c_str(), + "All mutex groups were already locked for [%s]", _context->requester_id().c_str()); _schedule(*_data.resume_itinerary); @@ -162,55 +181,16 @@ void LockMutexGroup::Active::_initialize() _schedule({rmf_traffic::Route(_data.hold_map, std::move(hold))}); _state->update_log().info( - "Waiting to lock mutex group [" + _data.mutex_group + "]"); + "Waiting to lock mutex group " + _data.all_groups_str()); RCLCPP_INFO( _context->node()->get_logger(), - "Waiting to lock mutex group [%s] for robot [%s]", - _data.mutex_group.c_str(), + "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); - // _listener = _context->node()->mutex_group_states() - // .observe_on(rxcpp::identity_same_worker(_context->worker())) - // .subscribe( - // [weak = weak_from_this()]( - // const MutexGroupStatesPtr& mutex_group_states) - // { - // const auto self = weak.lock(); - // if (!self) - // return; - - // for (const auto& assignment : mutex_group_states->assignments) - // { - // if (assignment.group == self->_data.mutex_group) - // { - // if (assignment.claimant == self->_context->participant_id()) - // { - // const auto finished = self->_finished; - // self->_finished = nullptr; - // if (finished) - // { - // std::cout << " === LOCKED MUTEX " << __LINE__ << std::endl; - // self->_state->update_status(State::Status::Completed); - // RCLCPP_INFO( - // self->_context->node()->get_logger(), - // "Requesting replan for robot [%s] after locking mutex group " - // "[%s]", - // self->_context->requester_id().c_str(), - // self->_data.mutex_group.c_str()); - - // // Things may have changed while waiting for the mutex to be - // // locked, so we should replan. - // self->_context->request_replan(); - // return; - // } - // } - // } - // } - // }); - _delay_timer = _context->node()->try_create_wall_timer( std::chrono::seconds(1), [weak = weak_from_this(), plan_id = *_data.plan_id]() @@ -225,48 +205,48 @@ void LockMutexGroup::Active::_initialize() }); std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() - << " TO " << _data.mutex_group << std::endl; - _listener = _context->request_mutex_group(_data.mutex_group, _data.hold_time) + << " TO " << _data.all_groups_str() << " AT " << _data.hold_position.transpose() << std::endl; + + _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 agv::MutexGroupSwitch& consider) + .subscribe([w = weak_from_this()](const std::string& locked) { const auto self = w.lock(); if (!self) return; - if (consider.to == self->_data.mutex_group) + self->_remaining.erase(locked); + if (self->_remaining.empty()) { const auto now = self->_context->now(); if (now - self->_data.hold_time > std::chrono::seconds(2)) { RCLCPP_INFO( self->_context->node()->get_logger(), - "Replanning for [%s] after a delay in locking mutex [%s]", + "Replanning for [%s] after a delay in locking mutexes %s", self->_context->requester_id().c_str(), - self->_data.mutex_group.c_str()); + self->_data.all_groups_str().c_str()); self->_context->request_replan(); self->_finished = nullptr; return; } // We locked the mutex quickly enough that we should proceed. - if (consider.accept()) + const auto finished = self->_finished; + self->_finished = nullptr; + if (finished) { - const auto finished = self->_finished; - self->_finished = nullptr; - if (finished) - { - RCLCPP_INFO( - self->_context->node()->get_logger(), - "Finished locking mutex [%s] for [%s]", - self->_data.mutex_group.c_str(), - self->_context->requester_id().c_str()); - - self->_schedule(*self->_data.resume_itinerary); - self->_state->update_status(Status::Completed); - finished(); - return; - } + 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->_state->update_status(Status::Completed); + finished(); + return; } } }); @@ -288,7 +268,7 @@ void LockMutexGroup::Active::_schedule( RCLCPP_ERROR( _context->node()->get_logger(), "Repeatedly failled attempts to update schedule during LockMutexGroup " - "action for robot [%s]. Last attempted value was [%s]. We will " + "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.", diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp index d1e2169f0..8777f4cb1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -26,18 +26,23 @@ 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::string mutex_group; + 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::string all_groups_str() const; }; class Standby : public rmf_task_sequence::Event::Standby @@ -96,6 +101,7 @@ class LockMutexGroup : public rmf_task_sequence::Event rclcpp::TimerBase::SharedPtr _delay_timer; std::shared_ptr _stubborn; Data _data; + std::unordered_set _remaining; }; }; 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 84cfc61fe..d6cf36f85 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -124,7 +124,7 @@ auto WaitForTraffic::Active::make( return; } - if (msg->group == self->_context->locked_mutex_group()) + 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 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 5ce3d1e4b..26aa2ccb4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -151,14 +151,8 @@ void DockRobot::Action::operator()(const Subscriber& s) if (wp.graph_index().has_value()) { const auto& graph = context->navigation_graph(); - if (graph.get_waypoint(*wp.graph_index()).in_mutex_group().empty()) - { - std::cout << "Releasing mutex group [" - << context->locked_mutex_group() << "] for [" - << context->requester_id() << "] after docking finished" - << std::endl; - context->release_mutex_group(); - } + context->retain_mutex_groups( + {graph.get_waypoint(*wp.graph_index()).in_mutex_group()}); } s.on_next(status); 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 ad684ecb9..49aada803 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -284,50 +284,41 @@ void MoveRobot::Action::operator()(const Subscriber& s) } } - if (!context->locked_mutex_group().empty()) + 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; + std::stringstream ss; + ss << " ############################# "; for (const auto& wp : self->_waypoints) { - if (wp.time() > adjusted_now) + 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) { - break; + ss << "[skip:" << s << "] "; + continue; } + ss << "[" << s << "] "; if (wp.graph_index().has_value()) { - if (self->_first_graph_index.has_value()) - { - const auto nav = self->_context->nav_params(); - const auto i_wp = *wp.graph_index(); - const auto i_first = *self->_first_graph_index; - bool ignore = - (nav && nav->in_same_stack(i_wp, i_first)) - || i_wp == i_first; - - if (ignore) - { - // The first waypoint doesn't always have a mutex group - // associated. - continue; - } - } - - const auto& g = graph.get_waypoint(*wp.graph_index()) - .in_mutex_group(); - if (g.empty()) - { - std::cout << __LINE__ << ": Releasing mutex at " << *wp.graph_index() - << " <" << wp.position().transpose() << ">" - << " | " << rmf_traffic::time::to_seconds(wp.time().time_since_epoch()) - << " vs " << rmf_traffic::time::to_seconds(adjusted_now.time_since_epoch()) - << std::endl; - context->release_mutex_group(); - break; - } + ss << graph.get_waypoint(*wp.graph_index()).in_mutex_group() << " "; + retain_mutexes.insert( + graph.get_waypoint(*wp.graph_index()).in_mutex_group()); + } + + for (const auto& l : wp.approach_lanes()) + { + ss << l << ":" << graph.get_lane(l).properties().in_mutex_group() + << " "; + retain_mutexes.insert( + graph.get_lane(l).properties().in_mutex_group()); } } + std::cout << ss.str() << std::endl; + context->retain_mutex_groups(retain_mutexes); } }); }; @@ -354,16 +345,8 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (last_index.has_value()) { const auto& graph = self->_context->navigation_graph(); - if (graph.get_waypoint(*last_index).in_mutex_group().empty()) - { - std::cout << __LINE__ << ": Releasing mutex at end of path" << std::endl; - self->_context->release_mutex_group(); - } - else - { - std::cout << __LINE__ << ": Not releasing because last waypoint still has mutex: " - << graph.get_waypoint(*last_index).in_mutex_group() << std::endl; - } + self->_context->retain_mutex_groups( + {graph.get_waypoint(*last_index).in_mutex_group()}); } } else From 09b453c0ec0273787cacedd4fc3ac26c3e19d549 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Thu, 23 Nov 2023 19:48:39 +0800 Subject: [PATCH 63/89] Debug logs and fix speed limit bug Signed-off-by: Xiyu Oh --- .../src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 3 +++ .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 5 +++-- .../src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp | 4 ++++ .../src/rmf_fleet_adapter/events/ExecutePlan.cpp | 2 +- .../src/rmf_fleet_adapter/events/GoToPlace.cpp | 1 + .../src/rmf_fleet_adapter/events/LockMutexGroup.cpp | 9 +++++++-- .../src/rmf_fleet_adapter/events/WaitForTraffic.cpp | 2 ++ .../src/rmf_fleet_adapter/phases/DockRobot.hpp | 4 ++++ .../src/rmf_fleet_adapter/phases/MoveRobot.hpp | 8 ++++++++ 9 files changed, 33 insertions(+), 5 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 a8315fcb7..6981bd66d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -639,6 +639,7 @@ auto EasyFullControl::CommandExecution::Implementation::make_hold( return; const auto delay = context->now() - expected_time; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, delay); if (const auto nav_params = context->nav_params()) { @@ -1497,6 +1498,7 @@ void EasyCommandHandle::dock( const auto dt = rmf_traffic::time::from_seconds(dist / v); const auto& itin = context->itinerary(); const auto now = context->now(); + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; 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; @@ -1532,6 +1534,7 @@ void EasyCommandHandle::dock( const rmf_traffic::Time now = context->now(); const auto updated_arrival = now + dt; const auto delay = updated_arrival - expected_arrival; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay( plan_id, delay, std::chrono::seconds(1)); }); 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 d4d0d0fcc..ff3fdc4d6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -98,10 +98,11 @@ inline std::string print_waypoint( //============================================================================== inline std::string print_plan_waypoint( const rmf_traffic::agv::Plan::Waypoint& wp, - const rmf_traffic::agv::Graph& graph) + 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().time_since_epoch()); + ss << "t=" << rmf_traffic::time::to_seconds(wp.time() - t0); if (wp.graph_index().has_value()) ss << " #" << *wp.graph_index(); ss << " <" << wp.position().transpose() 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 319c5109d..1f9f84e3a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -1085,6 +1085,7 @@ void ScheduleOverride::overridden_update( t_expected += rmf_traffic::time::from_seconds(s*dt); } const auto delay = now - t_expected; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, delay, delay_thresh); } else @@ -1107,6 +1108,7 @@ void ScheduleOverride::overridden_update( if (closest_time.has_value()) { const auto delay = now - closest_time->first; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, delay, delay_thresh); } @@ -1140,11 +1142,13 @@ void ScheduleOverride::overridden_update( // WARNING: We will have to change this implementation if it is ever // possible for the ScheduleOverride class to contain multiple routes in // its itinerary. + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; const auto cumulative_delay = context->itinerary() .cumulative_delay(plan_id).value_or(rmf_traffic::Duration(0)); plan_id = context->itinerary().assign_plan_id(); context->itinerary().set(plan_id, {route}); + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay( plan_id, cumulative_delay, delay_thresh); } 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 6d054c471..ced7961a1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -571,7 +571,7 @@ std::optional ExecutePlan::make( pss << "Plan waypoints"; for (const auto& wp : waypoints) { - pss << "\n -- " << agv::print_plan_waypoint(wp, graph); + pss << "\n -- " << agv::print_plan_waypoint(wp, graph, waypoints.front().time()); } std::cout << pss.str() << std::endl; 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 534e57f38..a0adc211c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -268,6 +268,7 @@ rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const const auto& itin = _context->itinerary(); if (_execution->plan_id) { + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; if (const auto delay = itin.cumulative_delay(*_execution->plan_id)) return finish - now + *delay; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index f06588100..f2cdd9368 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -16,6 +16,7 @@ */ #include "LockMutexGroup.hpp" +#include namespace rmf_fleet_adapter { namespace events { @@ -189,6 +190,7 @@ void LockMutexGroup::Active::_initialize() _context->requester_id().c_str()); const auto cumulative_delay = _context->now() - _data.hold_time; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); _delay_timer = _context->node()->try_create_wall_timer( @@ -201,6 +203,7 @@ void LockMutexGroup::Active::_initialize() const auto cumulative_delay = self->_context->now() - self->_data.hold_time; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; self->_context->itinerary().cumulative_delay(plan_id, cumulative_delay); }); @@ -220,12 +223,14 @@ void LockMutexGroup::Active::_initialize() if (self->_remaining.empty()) { const auto now = self->_context->now(); - if (now - self->_data.hold_time > std::chrono::seconds(2)) + const auto delay = now - self->_data.hold_time; + if (delay > std::chrono::seconds(2)) { RCLCPP_INFO( self->_context->node()->get_logger(), - "Replanning for [%s] after a delay in locking mutexes %s", + "Replanning for [%s] after a delay of %0.2fs in locking mutexes %s", self->_context->requester_id().c_str(), + rmf_traffic::time::to_seconds(delay), self->_data.all_groups_str().c_str()); self->_context->request_replan(); self->_finished = nullptr; 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 d6cf36f85..90d246b2d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -319,9 +319,11 @@ void WaitForTraffic::Active::_consider_going() return _replan(); } + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; const auto current_delay = _context->itinerary().cumulative_delay(_plan_id); if (current_delay.has_value() && *current_delay < cumulative_delay) { + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; _context->itinerary().cumulative_delay(_plan_id, cumulative_delay, 500ms); } } 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 26aa2ccb4..873d305fb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -155,6 +155,10 @@ void DockRobot::Action::operator()(const Subscriber& s) {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/MoveRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp index 49aada803..a80f66d3d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -195,8 +195,10 @@ void MoveRobot::Action::operator()(const Subscriber& s) ]( const auto&) { + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; if (const auto c = context->itinerary().cumulative_delay(plan_id)) { + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, *c + bump); } }); @@ -262,6 +264,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) << std::endl; const auto context = self->_context; const auto plan_id = self->_plan_id; + std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay( plan_id, new_cumulative_delay, 100ms); @@ -348,6 +351,11 @@ void MoveRobot::Action::operator()(const Subscriber& s) 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)); } else { From b960441a7575a6ddcbbd342d6342fdb1ce420667 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 23 Nov 2023 14:58:02 +0000 Subject: [PATCH 64/89] More robust decision making for replanning after mutex lock Signed-off-by: Michael X. Grey --- .../events/EmergencyPullover.cpp | 33 ++- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 8 +- .../rmf_fleet_adapter/events/ExecutePlan.hpp | 1 + .../rmf_fleet_adapter/events/GoToPlace.cpp | 2 +- .../events/LockMutexGroup.cpp | 201 +++++++++++++++--- .../events/LockMutexGroup.hpp | 12 +- 6 files changed, 219 insertions(+), 38 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 6b4f47287..222df2d6f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -336,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()) { 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 ced7961a1..4dc510133 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -520,6 +520,7 @@ std::optional ExecutePlan::make( agv::RobotContextPtr context, 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, @@ -664,6 +665,9 @@ std::optional ExecutePlan::make( } } + auto expected_waypoints = waypoints; + expected_waypoints.insert(expected_waypoints.begin(), wp); + previous_itinerary->erase( previous_itinerary->begin()+first_excluded_route, previous_itinerary->end()); @@ -676,7 +680,9 @@ std::optional ExecutePlan::make( hold_position, hold_time, plan_id, - next_itinerary + next_itinerary, + expected_waypoints, + goal }; previous_itinerary = data.resume_itinerary; 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 f19a0ee11..703349d35 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp @@ -34,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, 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 a0adc211c..e513fb16d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -597,7 +597,7 @@ void GoToPlace::Active::_execute_plan( } _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 index f2cdd9368..75313ec71 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -45,14 +45,13 @@ auto LockMutexGroup::Standby::make( Data data) -> std::shared_ptr { - auto standby = std::shared_ptr(new Standby); + 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()); - standby->_data = std::move(data); return standby; } @@ -76,6 +75,13 @@ auto LockMutexGroup::Standby::begin( 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, @@ -83,11 +89,10 @@ auto LockMutexGroup::Active::make( std::function finished, Data data) -> std::shared_ptr { - auto active = std::shared_ptr(new Active); + 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->_data = std::move(data); active->_initialize(); return active; @@ -140,6 +145,13 @@ void LockMutexGroup::Active::kill() cancel(); } +//============================================================================== +LockMutexGroup::Active::Active(Data data) +: _data(std::move(data)) +{ + // Do nothing +} + //============================================================================== void LockMutexGroup::Active::_initialize() { @@ -201,10 +213,7 @@ void LockMutexGroup::Active::_initialize() if (!self) return; - const auto cumulative_delay = - self->_context->now() - self->_data.hold_time; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; - self->_context->itinerary().cumulative_delay(plan_id, cumulative_delay); + self->_apply_cumulative_delay(); }); std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() @@ -222,37 +231,106 @@ void LockMutexGroup::Active::_initialize() 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)) { - RCLCPP_INFO( - self->_context->node()->get_logger(), - "Replanning for [%s] after a delay of %0.2fs in locking mutexes %s", - self->_context->requester_id().c_str(), - rmf_traffic::time::to_seconds(delay), - self->_data.all_groups_str().c_str()); - self->_context->request_replan(); - self->_finished = nullptr; - return; + 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. - const auto finished = self->_finished; - self->_finished = nullptr; - if (finished) - { - 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->_state->update_status(Status::Completed); - finished(); - return; - } + 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; } }); } @@ -302,5 +380,64 @@ void LockMutexGroup::Active::_schedule( } } +//============================================================================== +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 index 8777f4cb1..95a1ce944 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -19,10 +19,10 @@ #define SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP #include "../agv/RobotContext.hpp" +#include "../services/FindPath.hpp" #include - namespace rmf_fleet_adapter { namespace events { @@ -41,6 +41,8 @@ class LockMutexGroup : public rmf_task_sequence::Event 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; }; @@ -62,7 +64,7 @@ class LockMutexGroup : public rmf_task_sequence::Event std::function finished) final; private: - Standby() = default; + Standby(Data data); agv::RobotContextPtr _context; rmf_task::events::SimpleEventStatePtr _state; Data _data; @@ -92,8 +94,11 @@ class LockMutexGroup : public rmf_task_sequence::Event 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; @@ -102,6 +107,9 @@ class LockMutexGroup : public rmf_task_sequence::Event 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; }; }; From 02f9d6a83402460f8d35233fd8f679469287638e Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 24 Nov 2023 11:22:57 +0000 Subject: [PATCH 65/89] More robust lift release Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 5 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 168 +++++++++++++++--- .../rmf_fleet_adapter/events/GoToPlace.cpp | 64 ------- .../phases/EndLiftSession.cpp | 9 +- 4 files changed, 155 insertions(+), 91 deletions(-) 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 56e80cdd0..a8c22a539 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 @@ -459,6 +459,11 @@ rmf_traffic::agv::Graph parse_graph( } } + for (const auto& [_, lift] : lifts) + { + graph.known_lifts().insert(lift); + } + return graph; } 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 4dc510133..2674f5b1e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -515,6 +515,45 @@ class Printer : public rmf_traffic::agv::Graph::Lane::Executor void execute(const Dock& dock) override { std::cout << "event " << __LINE__ << std::endl;; } }; +//============================================================================== +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; + + 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 (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; + } +}; + //============================================================================== std::optional ExecutePlan::make( agv::RobotContextPtr context, @@ -543,12 +582,72 @@ std::optional ExecutePlan::make( // } // } + const auto& graph = context->navigation_graph(); + 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()) + { + std::cout << " :::::::::: " << context->requester_id() << " STILL HAVE LIFT LOCKED: " << current_lift->lift_name << std::endl; + 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 l_it = std::find_if( + graph.known_lifts().begin(), + graph.known_lifts().end(), + [&](const auto& lift) + { + return current_lift->lift_name == lift->name(); + }); + + if (l_it == graph.known_lifts().end()) + { + std::cout << "KNOWN LIFTS:"; + for (const auto& l : graph.known_lifts()) + { + std::cout << "\n -- " << l->name(); + } + std::cout << std::endl; + + RCLCPP_ERROR( + context->node()->get_logger(), + "Robot [%s] might be stuck with locking an unknown lift named [%s]", + context->requester_id().c_str(), + current_lift->lift_name.c_str()); + } + else + { + 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 = *l_it; + } + } + else + { + std::cout << " ::::::::::::: " << context->requester_id() << " STILL USING THE LIFT: " << current_lift->lift_name << std::endl; + } + } + auto plan_id = std::make_shared(recommended_plan_id); auto initial_itinerary = std::make_shared(full_itinerary); auto previous_itinerary = initial_itinerary; - const auto& graph = context->navigation_graph(); std::optional finish_time_estimate; for (const auto& r : plan.get_itinerary()) @@ -830,7 +929,11 @@ std::optional ExecutePlan::make( move_through.push_back(*it); - if (it->event()) + 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() || release_lift_here) { if (move_through.size() > 1) { @@ -840,6 +943,18 @@ std::optional ExecutePlan::make( it->time(), it->dependencies(), current_mutex_groups); } + 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; + } + std::optional next_waypoint; auto next_it = it + 1; if (next_it != waypoints.end()) @@ -849,35 +964,38 @@ std::optional ExecutePlan::make( move_through.clear(); bool continuous = true; - EventPhaseFactory factory( - context, legacy_phases, *it, next_waypoint, plan_id, - current_mutex_groups, make_current_mutex_groups, get_new_mutex_groups, - 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, + current_mutex_groups, make_current_mutex_groups, get_new_mutex_groups, + 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(); - - if (dist < 0.5) + const auto last_it = it; + ++it; + if (!it->event()) { - // We'll assume that this is just a misalignment in the maps - continue; + 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 + 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."); } - 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); } - - factory.initial_waypoint = *it; - it->event()->execute(factory); } if (continuous) 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 e513fb16d..17991633e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -498,45 +498,6 @@ void GoToPlace::Active::_schedule_retry() }); } -//============================================================================== -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; - - 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 (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; - } -}; - //============================================================================== void GoToPlace::Active::_execute_plan( const rmf_traffic::PlanId plan_id, @@ -571,31 +532,6 @@ void GoToPlace::Active::_execute_plan( _goal.waypoint(), _context->requester_id().c_str()); - 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) - { - RCLCPP_INFO( - _context->node()->get_logger(), - "Robot [%s] is releasing lift [%s] after a replan because it is no " - "longer needed.", - _context->requester_id().c_str(), - current_lift->lift_name.c_str()); - _context->release_lift(); - } - } - _execution = ExecutePlan::make( _context, plan_id, std::move(plan), _goal, std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); 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 fa2f414dd..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,12 @@ 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(); } @@ -119,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 + "]"; } //============================================================================== From 6485ffe5377955582bba117b26308f68b415aee1 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 24 Nov 2023 16:28:16 +0000 Subject: [PATCH 66/89] More robust door holding and releasing Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/TaskManager.cpp | 1 + .../rmf_fleet_adapter/agv/RobotContext.cpp | 62 +++++++- .../rmf_fleet_adapter/agv/RobotContext.hpp | 25 +++ .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 5 - .../rmf_fleet_adapter/events/ExecutePlan.cpp | 150 +++++++++++++++--- .../rmf_fleet_adapter/phases/DoorClose.cpp | 6 + .../src/rmf_fleet_adapter/phases/DoorOpen.cpp | 6 + 7 files changed, 225 insertions(+), 30 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index ee8215ba3..4a3e5fe19 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -100,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( 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 91e32ed06..2a38d1c86 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -22,6 +22,7 @@ #include #include +#include #include @@ -1029,6 +1030,13 @@ std::shared_ptr RobotContext::set_lift_destination( void RobotContext::release_lift() { _lift_destination = nullptr; + _initial_time_idle_outside_lift = std::nullopt; +} + +//============================================================================== +const std::optional& RobotContext::holding_door() const +{ + return _holding_door; } //============================================================================== @@ -1147,6 +1155,19 @@ void RobotContext::_set_charging(std::size_t wp, bool 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) @@ -1159,8 +1180,7 @@ void RobotContext::_check_lift_state( // 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. - _lift_destination = nullptr; - _initial_time_idle_outside_lift = std::nullopt; + release_lift(); } else if (_lift_destination && !_current_task_id.has_value()) { @@ -1194,8 +1214,7 @@ void RobotContext::_check_lift_state( _lift_destination->lift_name.c_str(), requester_id().c_str()); } - _lift_destination = nullptr; - _initial_time_idle_outside_lift = std::nullopt; + release_lift(); } else { @@ -1233,6 +1252,41 @@ void RobotContext::_check_lift_state( _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) 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 ff3fdc4d6..db110a387 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -620,6 +620,9 @@ class RobotContext /// 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; @@ -647,6 +650,12 @@ class RobotContext /// 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) { @@ -664,6 +673,17 @@ class RobotContext 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) @@ -774,6 +794,11 @@ class RobotContext rmf_rxcpp::subscription_guard _lift_subscription; std::optional _initial_time_idle_outside_lift; + 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, 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 24fa34ea7..e7630e9d5 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 @@ -458,11 +458,6 @@ rmf_traffic::agv::Graph parse_graph( } } - for (const auto& [_, lift] : lifts) - { - graph.known_lifts().insert(lift); - } - return graph; } 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 2674f5b1e..6de952517 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -554,6 +554,37 @@ class LiftFinder : public rmf_traffic::agv::Graph::Lane::Executor } }; +//============================================================================== +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, @@ -567,6 +598,8 @@ std::optional ExecutePlan::make( std::function finished, std::optional tail_period) { + if (plan.get_waypoints().empty()) + return std::nullopt; // std::cout << " --- plan --- " << std::endl; // const auto t0 = plan.get_waypoints().front().time(); // for (const rmf_traffic::agv::Plan::Waypoint& wp : plan.get_waypoints()) @@ -583,6 +616,8 @@ std::optional ExecutePlan::make( // } 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; @@ -602,18 +637,22 @@ std::optional ExecutePlan::make( if (!finder.still_using) { - const auto l_it = std::find_if( - graph.known_lifts().begin(), - graph.known_lifts().end(), - [&](const auto& lift) - { - return current_lift->lift_name == lift->name(); - }); + 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()); - if (l_it == graph.known_lifts().end()) + release_lift = found_lift; + } + else { std::cout << "KNOWN LIFTS:"; - for (const auto& l : graph.known_lifts()) + for (const auto& l : graph.all_known_lifts()) { std::cout << "\n -- " << l->name(); } @@ -625,21 +664,74 @@ std::optional ExecutePlan::make( context->requester_id().c_str(), current_lift->lift_name.c_str()); } - else + } + else + { + std::cout << " ::::::::::::: " << context->requester_id() << " STILL USING THE LIFT: " << current_lift->lift_name << std::endl; + } + } + + 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 first_graph_wp = [&]() -> std::optional { - 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()); + for (const auto& wp : plan.get_waypoints()) + { + if (wp.graph_index().has_value()) + return *wp.graph_index(); + } + + return std::nullopt; + }(); - release_lift = *l_it; + 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)) + { + legacy_phases.emplace_back( + std::make_shared( + context, + door->name(), + context->requester_id(), + plan.get_waypoints().front().time()), + plan.get_waypoints().front().time(), + rmf_traffic::Dependencies(), std::nullopt); + } } } - else + } + + 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()) { - std::cout << " ::::::::::::: " << context->requester_id() << " STILL USING THE LIFT: " << current_lift->lift_name << std::endl; + 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."); + release_door = current_door; } } @@ -863,7 +955,6 @@ std::optional ExecutePlan::make( return std::make_pair(mutex_group_change, new_mutex_groups); }; - LegacyPhases legacy_phases; while (!waypoints.empty()) { auto it = waypoints.begin(); @@ -933,7 +1024,10 @@ std::optional ExecutePlan::make( it->graph_index().has_value() && !release_lift->is_in_lift(it->position().block<2, 1>(0, 0), envelope); - if (it->event() || release_lift_here) + 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) { @@ -955,6 +1049,18 @@ std::optional ExecutePlan::make( 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()) @@ -1072,12 +1178,14 @@ std::optional ExecutePlan::make( if (const auto door = search_for_door_group(head, end, context, plan_id, event_id)) { + ss << "\n -- Move through door"; standbys.push_back(door->group); head = door->tail; } else if (const auto lift = search_for_lift_group( head, end, context, plan_id, event_id, state)) { + ss << "\n -- Move through lift"; standbys.push_back(lift->group); head = lift->tail; } 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()); } //============================================================================== From bcea1107dda98f3f732e3755ad958008b4e16230 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 25 Nov 2023 08:28:08 +0000 Subject: [PATCH 67/89] Ensure doors open after an interrupted plan Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 3 -- .../agv/RobotUpdateHandle.cpp | 26 +++++----- .../agv/internal_RobotUpdateHandle.hpp | 9 ---- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 31 +++++++++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 51 ++++++++++++++++--- .../rmf_fleet_adapter/events/GoToPlace.cpp | 1 - .../events/LockMutexGroup.cpp | 1 - .../events/WaitForTraffic.cpp | 2 - .../rmf_fleet_adapter/phases/MoveRobot.hpp | 3 -- 9 files changed, 88 insertions(+), 39 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 ff0c67a22..b7f5f1526 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -639,7 +639,6 @@ auto EasyFullControl::CommandExecution::Implementation::make_hold( return; const auto delay = context->now() - expected_time; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, delay); if (const auto nav_params = context->nav_params()) { @@ -1498,7 +1497,6 @@ void EasyCommandHandle::dock( const auto dt = rmf_traffic::time::from_seconds(dist / v); const auto& itin = context->itinerary(); const auto now = context->now(); - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; 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; @@ -1534,7 +1532,6 @@ void EasyCommandHandle::dock( const rmf_traffic::Time now = context->now(); const auto updated_arrival = now + dt; const auto delay = updated_arrival - expected_arrival; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay( plan_id, delay, std::chrono::seconds(1)); }); 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 1f9f84e3a..ca6c78973 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -245,14 +245,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; @@ -1085,7 +1091,6 @@ void ScheduleOverride::overridden_update( t_expected += rmf_traffic::time::from_seconds(s*dt); } const auto delay = now - t_expected; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, delay, delay_thresh); } else @@ -1108,7 +1113,6 @@ void ScheduleOverride::overridden_update( if (closest_time.has_value()) { const auto delay = now - closest_time->first; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, delay, delay_thresh); } @@ -1142,13 +1146,11 @@ void ScheduleOverride::overridden_update( // WARNING: We will have to change this implementation if it is ever // possible for the ScheduleOverride class to contain multiple routes in // its itinerary. - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; const auto cumulative_delay = context->itinerary() .cumulative_delay(plan_id).value_or(rmf_traffic::Duration(0)); plan_id = context->itinerary().assign_plan_id(); context->itinerary().set(plan_id, {route}); - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay( plan_id, cumulative_delay, delay_thresh); } 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 40b364f96..02baca36f 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 @@ -50,10 +50,8 @@ class TriggerOnce void trigger() const { - std::cout << " !!! " << __LINE__ << " triggering" << std::endl; if (!_callback) { - std::cout << " !!! " << __LINE__ << " no callback given" << std::endl; return; } @@ -62,16 +60,9 @@ class TriggerOnce std::shared_ptr> inner = *_callback; if (inner) { - std::cout << " !!! " << __LINE__ << " have inner" << std::endl; if (*inner) { - std::cout << " !!! " << __LINE__ << " triggering inner" << std::endl; (*inner)(); - std::cout << " !!! " << __LINE__ << " done triggering inner" << std::endl; - } - else - { - std::cout << " !!! " << __LINE__ << " inner was empty" << std::endl; } *inner = nullptr; 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 e7630e9d5..c42ea38a4 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 @@ -72,6 +72,37 @@ rmf_traffic::agv::Graph parse_graph( } } + 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) { 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 6de952517..e191f1db8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -615,6 +615,7 @@ std::optional ExecutePlan::make( // } // } + auto plan_id = std::make_shared(recommended_plan_id); const auto& graph = context->navigation_graph(); LegacyPhases legacy_phases; @@ -675,6 +676,9 @@ std::optional ExecutePlan::make( { const Eigen::Vector2d p0 = plan.get_waypoints().front().position().block<2, 1>(0, 0); + const auto t0 = plan.get_waypoints().front().time(); + + std::cout << " === " << __LINE__ << " broken start" << std::endl; const auto first_graph_wp = [&]() -> std::optional { for (const auto& wp : plan.get_waypoints()) @@ -688,6 +692,7 @@ std::optional ExecutePlan::make( if (first_graph_wp.has_value()) { + std::cout << " === " << __LINE__ << " merging onto " << *first_graph_wp << std::endl; const Eigen::Vector2d p1 = graph.get_waypoint(*first_graph_wp).get_location(); @@ -695,16 +700,45 @@ std::optional ExecutePlan::make( // 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(), - plan.get_waypoints().front().time()), - plan.get_waypoints().front().time(), - rmf_traffic::Dependencies(), std::nullopt); + 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, t0, side, plan_id), + t0, rmf_traffic::Dependencies(), std::nullopt); } } } @@ -730,12 +764,13 @@ std::optional ExecutePlan::make( RCLCPP_INFO( context->node()->get_logger(), "Robot [%s] will release door [%s] after a replan because it is no " - "longer needed."); + "longer needed.", + context->requester_id().c_str(), + current_door.c_str()); release_door = current_door; } } - auto plan_id = std::make_shared(recommended_plan_id); auto initial_itinerary = std::make_shared(full_itinerary); auto previous_itinerary = initial_itinerary; 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 17991633e..b8c504fa7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -268,7 +268,6 @@ rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const const auto& itin = _context->itinerary(); if (_execution->plan_id) { - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; if (const auto delay = itin.cumulative_delay(*_execution->plan_id)) return finish - now + *delay; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index 75313ec71..88d8fd12e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -202,7 +202,6 @@ void LockMutexGroup::Active::_initialize() _context->requester_id().c_str()); const auto cumulative_delay = _context->now() - _data.hold_time; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); _delay_timer = _context->node()->try_create_wall_timer( 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 90d246b2d..d6cf36f85 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -319,11 +319,9 @@ void WaitForTraffic::Active::_consider_going() return _replan(); } - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; const auto current_delay = _context->itinerary().cumulative_delay(_plan_id); if (current_delay.has_value() && *current_delay < cumulative_delay) { - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; _context->itinerary().cumulative_delay(_plan_id, cumulative_delay, 500ms); } } 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 a80f66d3d..02640dd89 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -195,10 +195,8 @@ void MoveRobot::Action::operator()(const Subscriber& s) ]( const auto&) { - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; if (const auto c = context->itinerary().cumulative_delay(plan_id)) { - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay(plan_id, *c + bump); } }); @@ -264,7 +262,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) << std::endl; const auto context = self->_context; const auto plan_id = self->_plan_id; - std::cout << __FILE__ << ": " << __LINE__ << "!!!!!" << std::endl; context->itinerary().cumulative_delay( plan_id, new_cumulative_delay, 100ms); From 4d69f2eb0d17af23dae63e938fc6729c6d64a605 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 25 Nov 2023 11:39:56 +0000 Subject: [PATCH 68/89] Account for waypoint merge radius when dealing with stacked waypoints Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 31 +++++++++++++++++-- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 7 +++++ 2 files changed, 35 insertions(+), 3 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 2a38d1c86..40ec352a9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -142,8 +142,10 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( const rmf_traffic::agv::Graph& graph, rmf_traffic::agv::Plan::StartSet locations) const { - for (rmf_traffic::agv::Plan::Start& location : locations) + 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()) { @@ -204,11 +206,34 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( // std::cout << "Descended vertex stack to " << waypoint << std::endl; if (waypoint != original_waypoint) { - location.lane(std::nullopt); - location.waypoint(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; } 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 c42ea38a4..b04d0da1c 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 @@ -223,6 +223,12 @@ rmf_traffic::agv::Graph parse_graph( } } } + + 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"]; @@ -368,6 +374,7 @@ rmf_traffic::agv::Graph parse_graph( 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}, From 49212465dd2ad10c608734ab49f6c9e77ff30a0c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sat, 25 Nov 2023 13:11:06 +0000 Subject: [PATCH 69/89] Fix mutex group decay Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 82 ++++++++++--------- 1 file changed, 45 insertions(+), 37 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 e191f1db8..4205010b8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -804,11 +804,13 @@ std::optional ExecutePlan::make( std::vector move_through; std::optional current_mutex_groups; + std::unordered_set remaining_mutex_groups; 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; @@ -843,37 +845,37 @@ std::optional ExecutePlan::make( } std::size_t first_excluded_route = 0; - // std::stringstream ss; - // if (current_mutex_groups.has_value()) - // { - // ss << "truncating for switch from [" - // << current_mutex_groups->all_groups_str() - // << "] to [" << new_mutex_groups << "] for [" - // << context->requester_id() << "]"; - // } - // else - // { - // ss << "truncating to lock [" << new_mutex_groups << "] for [" - // << context->requester_id() << "]"; - // } + std::stringstream ss; + if (current_mutex_groups.has_value()) + { + ss << "truncating for switch from [" + << current_mutex_groups->all_groups_str() + << "] to [" << all_str(new_mutex_groups) << "] for [" + << context->requester_id() << "]"; + } + else + { + ss << "truncating to lock " << all_str(new_mutex_groups) << " for [" + << context->requester_id() << "]"; + } 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(); - // ss << "\n -- erasing from checkpoint " << c.checkpoint_id - // << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; + ss << "\n -- erasing from checkpoint " << c.checkpoint_id + << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; - // ss << " t.size() before: " << t.size(); + ss << " t.size() before: " << t.size(); t.erase(t.begin() + (int)c.checkpoint_id, t.end()); - // ss << " vs after: " << t.size(); + ss << " vs after: " << t.size(); - // if (t.size() > 0) - // { - // ss << " ending at <" << t.back().position().transpose() << ">"; - // } + if (t.size() > 0) + { + ss << " ending at <" << t.back().position().transpose() << ">"; + } } - // std::cout << ss.str() << std::endl; + std::cout << ss.str() << std::endl; for (std::size_t i=0; i < previous_itinerary->size(); ++i) { @@ -957,36 +959,39 @@ std::optional ExecutePlan::make( << " | " << ss.str() << std::endl; bool mutex_group_change = - (!new_mutex_groups.empty() && !current_mutex_groups.has_value()); + (!new_mutex_groups.empty() && remaining_mutex_groups.empty()); + + std::cout << __LINE__ << " | " << !new_mutex_groups.empty() + << " " << !current_mutex_groups.has_value() << std::endl; - if (!mutex_group_change && current_mutex_groups.has_value()) + if (!mutex_group_change && !remaining_mutex_groups.empty()) { for (const auto& new_group : new_mutex_groups) { - if (current_mutex_groups->mutex_groups.count(new_group) == 0) + if (remaining_mutex_groups.count(new_group) == 0) { + std::cout << " === " << __LINE__ << " new group: " << new_group << std::endl; mutex_group_change = true; break; } + else + { + std::cout << " === " << __LINE__ << " old group: " << new_group << std::endl; + } } } - if (!mutex_group_change && current_mutex_groups.has_value()) + if (!mutex_group_change) { // We don't need to lock any new mutex groups, but we may be releasing - // some. We should reduce the current group set to whatever is in the - // new group set so that if we need to add more groups later then we are - // ready to do so. - if (new_mutex_groups.empty()) - { - current_mutex_groups = std::nullopt; - } - else - { - current_mutex_groups->mutex_groups = new_mutex_groups; - } + // 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; } + std::cout << " === " << __LINE__ << " | mutex_group_changed: " + << mutex_group_change << std::endl; return std::make_pair(mutex_group_change, new_mutex_groups); }; @@ -1006,6 +1011,7 @@ std::optional ExecutePlan::make( { if (move_through.size() > 1) { + std::cout << " >>> " << __LINE__ << std::endl; auto next_mutex_group = make_current_mutex_groups( new_mutex_groups, move_through.back()); @@ -1039,12 +1045,14 @@ std::optional ExecutePlan::make( // through yet. Just set the current_mutex_groups from this point. if (move_through.empty()) { + std::cout << " >>> " << __LINE__ << std::endl; current_mutex_groups = make_current_mutex_groups( new_mutex_groups, *it); std::cout << " === " << __LINE__ << ": " << current_mutex_groups->all_groups_str() << std::endl; } else { + std::cout << " >>> " << __LINE__ << std::endl; assert(move_through.size() == 1); current_mutex_groups = make_current_mutex_groups( new_mutex_groups, move_through.back()); From 4e3b4f809b9de3e91b453909846b2704da07355b Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 04:58:09 +0000 Subject: [PATCH 70/89] Fix follow_new_path start identification Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 161 +++++++++--------- .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 +- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 29 +++- .../rmf_fleet_adapter/phases/MoveRobot.cpp | 8 + 4 files changed, 116 insertions(+), 86 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 b7f5f1526..e8ce1b8ae 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -927,24 +927,6 @@ void EasyCommandHandle::stop() this->handle_stop(activity_identifier); } -class Printer : public rmf_traffic::agv::Graph::Lane::Executor -{ -public: - Printer() - { - // Do nothing - } - - void execute(const DoorOpen&) override { std::cout << __LINE__; } - void execute(const DoorClose&) override { std::cout << __LINE__; } - void execute(const LiftSessionBegin&) override { std::cout << __LINE__; } - void execute(const LiftDoorOpen&) override { std::cout << __LINE__; } - void execute(const LiftSessionEnd&) override { std::cout << __LINE__; } - void execute(const LiftMove&) override { std::cout << __LINE__; } - void execute(const Wait&) override { std::cout << __LINE__; } - void execute(const Dock& dock) override { std::cout << __LINE__; } -}; - //============================================================================== void EasyCommandHandle::follow_new_path( const std::vector& cmd_waypoints, @@ -986,33 +968,39 @@ void EasyCommandHandle::follow_new_path( } const auto& graph = planner->get_configuration().graph(); std::vector waypoints = cmd_waypoints; - std::size_t i_flatten = 1; - while (i_flatten < waypoints.size()) - { - const auto& wp0 = waypoints[i_flatten-1]; - const auto& wp1 = waypoints[i_flatten]; - if (!wp0.graph_index().has_value() || !wp1.graph_index().has_value()) - { - ++i_flatten; - continue; - } - - const auto i0 = *wp0.graph_index(); - const auto i1 = *wp1.graph_index(); - if (nav_params->in_same_stack(i0, i1)) - { - const auto yaw_diff = rmf_utils::wrap_to_pi( - wp0.position()[2] - wp1.position()[2]); - if (std::abs(yaw_diff) < 5*M_PI/180.0) - { - // These waypoints are redundant for moving so let's filter one out - waypoints.erase(waypoints.begin() + i_flatten); - continue; - } - } - - ++i_flatten; - } + // std::size_t i_flatten = 1; + // while (i_flatten < waypoints.size()) + // { + // const auto& wp0 = waypoints[i_flatten-1]; + // const auto& wp1 = waypoints[i_flatten]; + // if (!wp0.graph_index().has_value() && !wp1.graph_index().has_value()) + // { + // waypoints.erase(waypoints.begin() + i_flatten); + // continue; + // } + + // if (!wp0.graph_index().has_value() || !wp1.graph_index().has_value()) + // { + // ++i_flatten; + // continue; + // } + + // const auto i0 = *wp0.graph_index(); + // const auto i1 = *wp1.graph_index(); + // if (nav_params->in_same_stack(i0, i1)) + // { + // const auto yaw_diff = rmf_utils::wrap_to_pi( + // wp0.position()[2] - wp1.position()[2]); + // if (std::abs(yaw_diff) < 5*M_PI/180.0) + // { + // // These waypoints are redundant for moving so let's filter one out + // waypoints.erase(waypoints.begin() + i_flatten); + // continue; + // } + // } + + // ++i_flatten; + // } if (waypoints.size() < 2) { @@ -1066,13 +1054,13 @@ void EasyCommandHandle::follow_new_path( if (l.lane().has_value()) { ss << "lane[" << *l.lane() << "] "; - Printer printer; + EventPrinter printer; const auto& lane = graph.get_lane(*l.lane()); if (lane.entry().event()) { ss << " [entry "; lane.entry().event()->execute(printer); - ss << "] "; + ss << printer.text << "] "; } const auto& i_wp0 = lane.entry().waypoint_index(); const auto& wp0 = graph.get_waypoint(i_wp0); @@ -1084,7 +1072,7 @@ void EasyCommandHandle::follow_new_path( { ss << "[exit "; lane.exit().event()->execute(printer); - ss << "]"; + ss << printer.text << "]"; } ss << " | "; } @@ -1096,32 +1084,40 @@ void EasyCommandHandle::follow_new_path( return ss.str(); }; - std::cout << context->requester_id() << " Locations: " << current_location.size() << std::endl; + std::stringstream ss; + ss << context->requester_id() << " Locations: " << current_location.size(); for (const auto& l : current_location) { - std::cout << " -- " << print_location(l) << std::endl; + ss << "\n -- " << print_location(l); } - std::cout << "Num waypoints: " << waypoints.size() << std::endl; + ss << "\nNum waypoints: " << waypoints.size(); for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) { - std::cout << " --"; - if (wp.graph_index().has_value()) - { - std::cout << " index " << *wp.graph_index(); - const auto& gwp = graph.get_waypoint(*wp.graph_index()); - if (!gwp.in_mutex_group().empty()) - { - std::cout << " *" << gwp.in_mutex_group(); - } - - std::cout << " [" << gwp.get_map_name() << "]"; - } - std::cout << " " << wp.position().transpose() << std::endl; + ss << "\n -- " << print_plan_waypoint(wp, graph, waypoints.front().time()); + // if (wp.graph_index().has_value()) + // { + // ss << " index " << *wp.graph_index(); + // const auto& gwp = graph.get_waypoint(*wp.graph_index()); + // if (!gwp.in_mutex_group().empty()) + // { + // ss << " *" << gwp.in_mutex_group(); + // } + + // ss << " [" << gwp.get_map_name() << "]"; + // } + // ss << " " << wp.position().transpose(); + + // ss << " approach lanes:"; + // for (const auto& l : wp.approach_lanes()) + // { + // ss << " " << l; + // } } + std::cout << ss.str() << std::endl; bool found_connection = false; std::size_t i0 = 0; - std::stringstream ss; + ss = std::stringstream(); ss << "Finding connection for " << context->requester_id(); for (std::size_t i = 0; i < waypoints.size(); ++i) { @@ -1191,12 +1187,16 @@ void EasyCommandHandle::follow_new_path( if (i > 0) { + std::stringstream ss; + ss << "approach lanes:"; for (const auto lane : wp.approach_lanes()) { + ss << " " << lane; for (const auto& l : current_location) { if (l.lane().has_value()) { + ss << " [vs " << *l.lane() << "]"; if (lane == *l.lane()) { found_connection = true; @@ -1205,6 +1205,7 @@ void EasyCommandHandle::follow_new_path( } } } + std::cout << ss.str() << std::endl; } } @@ -1289,23 +1290,25 @@ 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) { - const auto gi_1 = *wp1.graph_index(); - const auto gi_2 = *wp2.graph_index(); - if (nav_params->in_same_stack(gi_1, gi_2)) + 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; } 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 db110a387..bfdd2ca5c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -123,13 +123,15 @@ inline std::string print_plan_waypoint( } } + 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 << " lane mutex [" << m << "]"; + ss << " [" << m << "]"; } } 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 4205010b8..24682c2b3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -40,6 +40,16 @@ 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 { @@ -111,7 +121,6 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor const rmf_traffic::agv::Plan::Waypoint& initial_waypoint_, std::optional next_waypoint_, const PlanIdPtr plan_id, - std::optional& current_mutex_group, std::function&, const rmf_traffic::agv::Plan::Waypoint&)> make_current_mutex_groups, @@ -124,7 +133,6 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _phases(phases), _event_start_time(initial_waypoint_.time()), _plan_id(plan_id), - _current_mutex_group(current_mutex_group), _make_current_mutex_groups(std::move(make_current_mutex_groups)), _get_new_mutex_group(std::move(get_new_mutex_groups)), _continuous(continuous) @@ -147,7 +155,6 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor { event_mutex_group = _make_current_mutex_groups( new_mutex_groups, initial_waypoint); - _current_mutex_group = event_mutex_group; } } @@ -272,7 +279,6 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor LegacyPhases& _phases; rmf_traffic::Time _event_start_time; PlanIdPtr _plan_id; - std::optional& _current_mutex_group; std::function&, const rmf_traffic::agv::Plan::Waypoint&)> _make_current_mutex_groups; @@ -955,7 +961,7 @@ std::optional ExecutePlan::make( } } - std::cout << " === " << __LINE__ << " " << all_str(new_mutex_groups) + std::cout << " ===>>> " << __LINE__ << " " << all_str(new_mutex_groups) << " | " << ss.str() << std::endl; bool mutex_group_change = @@ -1029,6 +1035,7 @@ std::optional ExecutePlan::make( move_through.clear(); // Repeat the last waypoint so that follow_new_path has continuity. move_through.push_back(last); + std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph); waypoints.erase(waypoints.begin(), it); @@ -1062,6 +1069,7 @@ std::optional ExecutePlan::make( } move_through.push_back(*it); + std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph) << std::endl; const bool release_lift_here = release_lift && it->graph_index().has_value() && @@ -1079,6 +1087,13 @@ std::optional ExecutePlan::make( context, move_through, plan_id, tail_period), 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) { @@ -1117,7 +1132,7 @@ std::optional ExecutePlan::make( { EventPhaseFactory factory( context, legacy_phases, *it, next_waypoint, plan_id, - current_mutex_groups, make_current_mutex_groups, get_new_mutex_groups, + make_current_mutex_groups, get_new_mutex_groups, continuous); it->event()->execute(factory); while (factory.moving_lift()) @@ -1152,6 +1167,7 @@ std::optional ExecutePlan::make( // Have the next sequence of waypoints begin with the event waypoint // of this sequence. move_through.push_back(*it); + std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph) << std::endl; } waypoints.erase(waypoints.begin(), it+1); @@ -1176,6 +1192,7 @@ std::optional ExecutePlan::make( // Have the next sequence of waypoints begin with this one. move_through.clear(); move_through.push_back(*it); + std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph) << std::endl; waypoints.erase(waypoints.begin(), it+1); event_occurred = true; 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 aeabf6ea0..1bf0cf991 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -30,6 +30,14 @@ MoveRobot::ActivePhase::ActivePhase( : _context{std::move(context)}, _tail_period(tail_period) { + std::stringstream ss; + ss << "Activating MoveRobot:"; + for (const auto& wp : waypoints) + { + ss << "\n -- " << agv::print_plan_waypoint(wp, _context->navigation_graph(), waypoints.front().time()); + } + std::cout << ss.str() << std::endl; + std::ostringstream oss; const auto dest = destination( waypoints.back(), _context->planner()->get_configuration().graph()); From 058b3aad4522758f9cf1d73921ca823d8dd59517 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 11:09:35 +0000 Subject: [PATCH 71/89] Fix segfault during door/lift phases Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 42 +++----------- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 58 +++++++++++++------ .../rmf_fleet_adapter/events/GoToPlace.cpp | 10 +++- .../events/LockMutexGroup.cpp | 11 +++- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 40 +++++-------- .../rmf_fleet_adapter/project_itinerary.cpp | 12 +++- .../rmf_fleet_adapter/project_itinerary.hpp | 3 +- 7 files changed, 94 insertions(+), 82 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 e8ce1b8ae..e24e6c53f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -452,6 +452,7 @@ class EasyFullControl::CommandExecution::Implementation::Data if (starts.empty()) { + std::cout << __LINE__ << " " << context->requester_id() << " compute_plan_starts" << std::endl; starts = nav_params->compute_plan_starts(graph, map, location, now); } @@ -1233,6 +1234,8 @@ void EasyCommandHandle::follow_new_path( i0 = waypoints.size() - 2; } + std::stringstream wss; + wss << " ----------- " << context->requester_id() << " Moving through:"; std::size_t i1 = i0 + 1; while (i1 < waypoints.size()) { @@ -1340,6 +1343,7 @@ void EasyCommandHandle::follow_new_path( in_lift); const auto target_p = waypoints.at(target_index).position(); + wss << "\n -- " << target_p.transpose(); queue.push_back( EasyFullControl::CommandExecution::Implementation::make( context, @@ -1369,26 +1373,11 @@ void EasyCommandHandle::follow_new_path( i1 = i0 + 1; } - auto finisher = [path_finished_callback_]() - { - std::cout << "CALLING THE PATH FINISHER" << std::endl; - if (path_finished_callback_) - { - path_finished_callback_(); - std::cout << "DONE CALLING THE PATH FINISHER" << std::endl; - } - else - { - std::cout << " ??? PATH FINISHED CALLBACK IS NULL" << std::endl; - } - }; + std::cout << wss.str() << std::endl; - std::cout << __LINE__ << ": SETTING CURRENT_PROGRESS FOR " << context->requester_id() - << " | " << waypoints.back().position().transpose() << std::endl; this->current_progress = ProgressTracker::make( queue, - // path_finished_callback_); - finisher); + path_finished_callback_); this->current_progress->next(); } @@ -1573,26 +1562,9 @@ void EasyCommandHandle::dock( handle_nav_request(destination, execution); }); - auto finisher = [docking_finished_callback_]() - { - std::cout << " CALLING THE DOCKING FINISHER" << std::endl; - if (docking_finished_callback_) - { - docking_finished_callback_(); - std::cout << " DONE CALLING THE DOCKING FINISHER" << std::endl; - } - else - { - std::cout << " ??? DOCKING FINISHED CALLBACK IS NULL" << std::endl; - } - }; - - std::cout << __LINE__ << ": SETTING CURRENT_PROGRESS FOR " << context->requester_id() - << " | " << command_position.transpose() << std::endl; this->current_progress = ProgressTracker::make( {std::move(cmd)}, - // std::move(docking_finished_callback_)); - finisher); + std::move(docking_finished_callback_)); this->current_progress->next(); } 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 24682c2b3..fa6573c85 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -344,12 +344,15 @@ std::optional search_for_door_group( context, id, it->mutex_group_dependency.value())); } - door_group.push_back( - [legacy = it->phase, context, id](UpdateFn update) - { - return LegacyPhaseShim::Standby::make( - legacy, context->worker(), context->clock(), id, update); - }); + 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()) { @@ -459,12 +462,15 @@ std::optional search_for_lift_group( context, event_id, it->mutex_group_dependency.value())); } - 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->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()) { @@ -806,6 +812,11 @@ std::optional ExecutePlan::make( { pss << "\n -- " << agv::print_plan_waypoint(wp, graph, waypoints.front().time()); } + pss << "\nItineraries:"; + for (const auto& r : full_itinerary) + { + pss << "\n -- " << r.map() << ":" << r.trajectory().size(); + } std::cout << pss.str() << std::endl; std::vector move_through; @@ -881,13 +892,13 @@ std::optional ExecutePlan::make( ss << " ending at <" << t.back().position().transpose() << ">"; } } - std::cout << ss.str() << std::endl; for (std::size_t i=0; i < previous_itinerary->size(); ++i) { const auto& t = previous_itinerary->at(i).trajectory(); if (t.size() < 2) { + ss << "\n -- excluding from " << i; // 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 @@ -906,6 +917,8 @@ std::optional ExecutePlan::make( previous_itinerary->begin()+first_excluded_route, previous_itinerary->end()); + std::cout << ss.str() << std::endl; + auto next_itinerary = std::make_shared< rmf_traffic::schedule::Itinerary>(full_itinerary); auto data = LockMutexGroup::Data{ @@ -1315,9 +1328,9 @@ std::optional ExecutePlan::make( RCLCPP_ERROR( context->node()->get_logger(), - "Invalid 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.", + "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(), @@ -1333,13 +1346,22 @@ std::optional ExecutePlan::make( 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]", + "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()); + task_id.c_str(), + initial_itinerary->size(), + ss_sizes.str().c_str()); return std::nullopt; } } 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 b8c504fa7..b4a4fb46c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -438,8 +438,16 @@ void GoToPlace::Active::_find_plan() "Found a plan to move from [" + start_name + "] to [" + goal_name + "]"); + std::stringstream ss; + ss << self->_context->requester_id() << " initial itinerary: "; + for (const auto& r : result->get_itinerary()) + ss << "[" << r.map() << ":" << r.trajectory().size() << "]"; + + ss << " + "; auto full_itinerary = project_itinerary( - *result, self->_followed_by, *self->_context->planner()); + *result, self->_followed_by, *self->_context->planner(), &ss); + + std::cout << ss.str() << std::endl; self->_execute_plan( self->_context->itinerary().assign_plan_id(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index 88d8fd12e..a78faf5ba 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -347,13 +347,22 @@ void LockMutexGroup::Active::_schedule( { if (++attempts > 5) { + std::stringstream ss_sizes; + for (const auto& r : itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + RCLCPP_ERROR( _context->node()->get_logger(), - "Repeatedly failled attempts to update schedule during LockMutexGroup " + "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.", + itinerary.size(), + ss_sizes.str().c_str(), _context->requester_id().c_str(), *_data.plan_id); break; 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 02640dd89..988922b53 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -257,9 +257,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) const auto self = w.lock(); if (!self) return; - std::cout << "delaying " << self->_context->requester_id() << " plan id " - << self->_plan_id << " by " << rmf_traffic::time::to_seconds(new_cumulative_delay) - << std::endl; + const auto context = self->_context; const auto plan_id = self->_plan_id; context->itinerary().cumulative_delay( @@ -286,38 +284,44 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (!context->locked_mutex_groups().empty()) { + // std::stringstream ss; + // ss << context->requester_id() << " retaining:"; const auto adjusted_now = now - new_cumulative_delay; const auto& graph = context->navigation_graph(); std::unordered_set retain_mutexes; - std::stringstream ss; - ss << " ############################# "; for (const auto& wp : self->_waypoints) { 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) { - ss << "[skip:" << s << "] "; continue; } - ss << "[" << s << "] "; if (wp.graph_index().has_value()) { - ss << graph.get_waypoint(*wp.graph_index()).in_mutex_group() << " "; + // if (!graph.get_waypoint(*wp.graph_index()).in_mutex_group().empty()) + // { + // ss << " [wp:" << graph.get_waypoint(*wp.graph_index()).name_or_index() + // << " | " << graph.get_waypoint(*wp.graph_index()).in_mutex_group() << "]"; + // } retain_mutexes.insert( graph.get_waypoint(*wp.graph_index()).in_mutex_group()); } for (const auto& l : wp.approach_lanes()) { - ss << l << ":" << graph.get_lane(l).properties().in_mutex_group() - << " "; + // if (!graph.get_lane(l).properties().in_mutex_group().empty()) + // { + // ss << " [lane:" << l << " | " << << "]"; + // } + retain_mutexes.insert( graph.get_lane(l).properties().in_mutex_group()); } } - std::cout << ss.str() << std::endl; + + // std::cout << ss.str() << std::endl; context->retain_mutex_groups(retain_mutexes); } }); @@ -325,7 +329,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) const auto finish = [s, w = weak_from_this(), name = _context->requester_id()]() { - std::cout << "PATH FINISHER TRIGGERED" << std::endl; if (const auto self = w.lock()) { if (!self->_waypoints.empty()) @@ -334,11 +337,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) { self->_context->itinerary().reached( self->_plan_id, c.route_id, c.checkpoint_id); - - std::cout << "finish reached " << self->_context->participant_id() - << " | " << self->_context->itinerary().current_plan_id() - << ":" << c.route_id << ":" << c.checkpoint_id - << " #" << self->_context->itinerary().progress_version() << std::endl; } const auto last_index = self->_waypoints.back().graph_index(); @@ -354,10 +352,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) self->_context->itinerary().cumulative_delay( self->_plan_id, cumulative_delay, std::chrono::seconds(1)); } - else - { - std::cout << __LINE__ << " EMPTY WAYPOINTS??? FOR " << self->_context->requester_id() << std::endl; - } LegacyTask::StatusMsg msg; msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; @@ -365,10 +359,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) s.on_next(msg); s.on_completed(); } - else - { - std::cout << " ###### MOVE ROBOT IS PREMATURELY DEAD FOR " << name << std::endl; - } }; _context->command()->follow_new_path( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp index 55dd588b9..642ea4cc2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp @@ -23,7 +23,8 @@ namespace rmf_fleet_adapter { rmf_traffic::schedule::Itinerary project_itinerary( const rmf_traffic::agv::Plan& starting_from, const std::vector& through_destinations, - const rmf_traffic::agv::Planner& with_planner) + const rmf_traffic::agv::Planner& with_planner, + std::stringstream* ss) { auto itinerary = starting_from.get_itinerary(); auto last_plan = starting_from; @@ -49,6 +50,11 @@ rmf_traffic::schedule::Itinerary project_itinerary( const auto& new_itinerary = last_plan.get_itinerary(); if (new_itinerary.front().map() == itinerary.back().map()) { + if (ss) + { + *ss << "[" << new_itinerary.front().map() << ":" << new_itinerary.front().trajectory().size() + << "]"; + } // We only look at the first route because we're not going to include // any map switches for now. for (const auto& wp : new_itinerary.front().trajectory()) @@ -56,6 +62,10 @@ rmf_traffic::schedule::Itinerary project_itinerary( } else { + if (ss) + { + *ss << "[" << new_itinerary.front().map() << "rejected]"; + } // If the map has switched, let's break out of this loop. break; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp index c8fe77776..f6a40d2d2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp @@ -27,7 +27,8 @@ namespace rmf_fleet_adapter { rmf_traffic::schedule::Itinerary project_itinerary( const rmf_traffic::agv::Plan& starting_from, const std::vector& through_destinations, - const rmf_traffic::agv::Planner& with_planner); + const rmf_traffic::agv::Planner& with_planner, + std::stringstream* ss = nullptr); } // namespace rmf_fleet_adapter From 159f7aea1d3039fc6dc091fc5dc05cafbb3c4f5d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 13:12:52 +0000 Subject: [PATCH 72/89] Ensure follow_new_path is protected by the main worker Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 429 +++++++++--------- 1 file changed, 222 insertions(+), 207 deletions(-) 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 988922b53..71edf2357 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -139,245 +139,260 @@ 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()]() - { - 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(); - }); - - const auto update = [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; - - 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()) + _context->worker().schedule([w = weak_from_this(), s](const auto&) + { + const auto self = w.lock(); + if (!self) + return; + + 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 now = action->_context->now(); - if (!action->_last_tail_bump.has_value() - || *action->_last_tail_bump + *action->_tail_period < now) + const auto self = w.lock(); + if (!self) + return; + + const auto now = self->_context->node()->now(); + if (now < self->_last_update_rostime + self->_update_timeout) { - 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)) - { - context->itinerary().cumulative_delay(plan_id, *c + bump); - } - }); + // The simulation is paused or running slowly, so we should allow more + // patience before assuming that there's been a timeout. + return; } - } - if (path_index != action->_next_path_index) + 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(); + }); + + const auto update = [ + s, + w_action = self->weak_from_this(), + r = self->_context->requester_id() + ]( + std::size_t path_index, rmf_traffic::Duration estimate) { - action->_next_path_index = path_index; - LegacyTask::StatusMsg msg; - msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; + const auto action = w_action.lock(); + if (!action) + return; - if (path_index < action->_waypoints.size()) + 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()) { - msg.status = "Heading towards " - + destination( - action->_waypoints[path_index], - action->_context->planner()->get_configuration().graph()); + 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&) + { + if (const auto c = context->itinerary().cumulative_delay(plan_id)) + { + context->itinerary().cumulative_delay(plan_id, *c + bump); + } + }); + } } - else + + if (path_index != action->_next_path_index) { - // 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) + "]"; - } + action->_next_path_index = path_index; + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; - s.on_next(msg); - } + 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) + "]"; + } - if (action->_next_path_index > action->_waypoints.size()) - { - return; - } + s.on_next(msg); + } - 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( - [ - w = action->weak_from_this(), - now, - new_cumulative_delay - ](const auto&) + if (action->_next_path_index > action->_waypoints.size()) { - 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) - { - const auto& traj = itin[i].trajectory(); - const auto t_it = traj.find(now); - if (t_it != traj.end() && t_it != traj.begin()) - { - std::size_t index = t_it->index() - 1; - if (t_it->time() == now) - { - index = t_it->index(); - } + return; + } - context->itinerary().reached(plan_id, i, index); - } - } + 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; + } - if (!context->locked_mutex_groups().empty()) + 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&) { - // std::stringstream ss; - // ss << context->requester_id() << " retaining:"; - 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) + 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) { - 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) + const auto& traj = itin[i].trajectory(); + const auto t_it = traj.find(now); + if (t_it != traj.end() && t_it != traj.begin()) { - continue; - } + std::size_t index = t_it->index() - 1; + if (t_it->time() == now) + { + index = t_it->index(); + } - if (wp.graph_index().has_value()) - { - // if (!graph.get_waypoint(*wp.graph_index()).in_mutex_group().empty()) - // { - // ss << " [wp:" << graph.get_waypoint(*wp.graph_index()).name_or_index() - // << " | " << graph.get_waypoint(*wp.graph_index()).in_mutex_group() << "]"; - // } - retain_mutexes.insert( - graph.get_waypoint(*wp.graph_index()).in_mutex_group()); + context->itinerary().reached(plan_id, i, index); } + } - for (const auto& l : wp.approach_lanes()) + if (!context->locked_mutex_groups().empty()) + { + // std::stringstream ss; + // ss << context->requester_id() << " retaining:"; + 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) { - // if (!graph.get_lane(l).properties().in_mutex_group().empty()) - // { - // ss << " [lane:" << l << " | " << << "]"; - // } - - retain_mutexes.insert( - graph.get_lane(l).properties().in_mutex_group()); + 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()) + { + // if (!graph.get_waypoint(*wp.graph_index()).in_mutex_group().empty()) + // { + // ss << " [wp:" << graph.get_waypoint(*wp.graph_index()).name_or_index() + // << " | " << graph.get_waypoint(*wp.graph_index()).in_mutex_group() << "]"; + // } + retain_mutexes.insert( + graph.get_waypoint(*wp.graph_index()).in_mutex_group()); + } + + for (const auto& l : wp.approach_lanes()) + { + // if (!graph.get_lane(l).properties().in_mutex_group().empty()) + // { + // ss << " [lane:" << l << " | " << << "]"; + // } + + retain_mutexes.insert( + graph.get_lane(l).properties().in_mutex_group()); + } } - } - - // std::cout << ss.str() << std::endl; - context->retain_mutex_groups(retain_mutexes); - } - }); - }; - const auto finish = [s, w = weak_from_this(), name = _context->requester_id()]() - { - if (const auto self = w.lock()) + // std::cout << ss.str() << std::endl; + context->retain_mutex_groups(retain_mutexes); + } + }); + }; + + 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 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)); } - 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(); - } - }; - - _context->command()->follow_new_path( - _waypoints, - [worker = _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 = _context->worker(), finish]() - { - worker.schedule([finish](const auto&) - { - finish(); - }); - }); + 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(); + }); + }); + }); } } // namespace phases From 2dbe0fc61ff07f67f9d4b4154adcf4d1042b226a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 13:31:41 +0000 Subject: [PATCH 73/89] Make robots stubborn while using the lift Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 10 ++++++++++ .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 1 + 2 files changed, 11 insertions(+) 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 40ec352a9..67e48d1b7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1056,6 +1056,7 @@ void RobotContext::release_lift() { _lift_destination = nullptr; _initial_time_idle_outside_lift = std::nullopt; + _lift_stubbornness = nullptr; } //============================================================================== @@ -1259,6 +1260,15 @@ void RobotContext::_check_lift_state( 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(); + } + } } if (!_lift_destination) 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 bfdd2ca5c..1591214be 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -795,6 +795,7 @@ class RobotContext std::shared_ptr _lift_destination; rmf_rxcpp::subscription_guard _lift_subscription; std::optional _initial_time_idle_outside_lift; + std::shared_ptr _lift_stubbornness; void _check_door_supervisor( const rmf_door_msgs::msg::SupervisorHeartbeat& hb); From 810aa7486166e7d9d786d6e4a972d184dffc4c79 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 16:50:51 +0000 Subject: [PATCH 74/89] Generate task bids asynchronously Signed-off-by: Michael X. Grey --- .../agv/FleetUpdateHandle.cpp | 457 ++++++++++-------- .../agv/internal_FleetUpdateHandle.hpp | 49 +- 2 files changed, 276 insertions(+), 230 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 72ff7f493..37421607c 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,129 @@ 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 +469,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 +502,121 @@ 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); + + auto receive_allocation = [w = weak_self, respond, task_id]( + AllocateTasks::Result result) + { + const auto self = w.lock(); + if (!self) + return; + + auto allocation_result = result.assignments; + if (!allocation_result.has_value()) + return respond({std::nullopt, std::move(result.errors)}); - const auto& assignments = allocation_result.value(); + const auto& assignments = allocation_result.value(); - const double cost = task_planner->compute_cost(assignments); + const double cost = self->_pimpl->task_planner->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) - { - debug_stream << "--Agent: " << i << std::endl; - for (const auto& a : assignments[i]) + // 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) { - 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 << "--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; + debug_stream << " ----------------------" << std::endl; - RCLCPP_DEBUG(node->get_logger(), "%s", debug_stream.str().c_str()); + 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 : task_managers) - { - robot_name_map.insert({index, t.first->name()}); - ++index; - } + // 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& assignment : agent) + std::optional robot_name; + std::optional finish_time; + index = 0; + for (const auto& agent : assignments) { - if (assignment.request()->booking()->id() == task_id) + for (const auto& assignment : agent) { - finish_time = assignment.finish_state().time().value(); - if (robot_name_map.find(index) != robot_name_map.end()) - robot_name = robot_name_map[index]; - break; + 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; } - ++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.")); - - return respond({std::nullopt, std::move(errors)}); - } - // Publish BidProposal - respond( + if (!robot_name.has_value() || !finish_time.has_value()) { - rmf_task_ros2::bidding::Response::Proposal{ - name, - *robot_name, - current_assignment_cost, - cost, - *finish_time - }, - std::move(errors) - }); + 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.")); - 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); + return respond({std::nullopt, std::move(result.errors)}); + } - // Store assignments in internal map - bid_notice_assignments.insert({task_id, assignments}); + // 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}); + }; + + calculate_bid_subscription = rmf_rxcpp::make_job( + calculate_bid) + .observe_on(rxcpp::identity_same_worker(worker)) + .subscribe(receive_allocation); } //============================================================================== @@ -619,7 +748,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 +771,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(), - "TaskAssignments 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 +815,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; @@ -1350,106 +1487,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 { 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 9973178a1..04e9be790 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 @@ -233,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 { @@ -297,9 +309,6 @@ class FleetUpdateHandle::Implementation std::unordered_map unregistered_charging_assignments; - // Map task id to pair of - using TaskAssignments = rmf_task::TaskPlanner::Assignments; - using DockParamMap = std::unordered_map< std::string, @@ -345,6 +354,11 @@ 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) { @@ -607,22 +621,8 @@ 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(TaskAssignments& assignments) const; @@ -645,15 +645,24 @@ class FleetUpdateHandle::Implementation 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 From 5798519220f61f6b244cbd7aefdc1cec3f41b6d4 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 17:24:34 +0000 Subject: [PATCH 75/89] Aggressive end session Signed-off-by: Michael X. Grey --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 2166439a4..190f80e70 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -110,10 +110,10 @@ void Node::_lift_state_update(LiftState::UniquePtr msg) msg->session_id.c_str(), lift_request->destination_floor.c_str() ); } - else if (!msg->session_id.empty()) + else { - // The lift missed the publication of the session end request so we should - // republish it. + // 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; From 5be59393d7a3b345e29270120fe8adebc136d958 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Sun, 26 Nov 2023 18:33:33 +0000 Subject: [PATCH 76/89] Introduce mutex deconfliction strategy Signed-off-by: Michael X. Grey --- .../src/mutex_group_supervisor/main.cpp | 77 ++++++++++++++++++- 1 file changed, 76 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index a7856f5bc..b09a80984 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -102,7 +102,7 @@ class Node : public rclcpp::Node { if (s.group == request.group && s.claimant != Unclaimed) { - // The group is already claimed, so nothing to be done here + check_for_conflicts(); return; } } @@ -155,6 +155,81 @@ class Node : public rclcpp::Node 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]; From 6a2a7d4d24e50ac57073e9b298cea5f95c197e52 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 27 Nov 2023 04:45:24 +0000 Subject: [PATCH 77/89] Skip null events while moving through lift Signed-off-by: Michael X. Grey --- .../agv/RobotUpdateHandle.hpp | 3 ++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 8 ++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 2 +- .../agv/RobotUpdateHandle.cpp | 22 +++++++++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 37 +++++++++++++++---- .../rmf_fleet_adapter/phases/RequestLift.cpp | 5 +++ 6 files changed, 69 insertions(+), 8 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 bb6953484..22da7b444 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,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 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 67e48d1b7..57b440c05 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1054,6 +1054,14 @@ std::shared_ptr RobotContext::set_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; 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 1591214be..f8c14696a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -75,7 +75,7 @@ class EventPrinter : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftSessionBegin&) override { text = "LiftSessionBegin"; } void execute(const LiftDoorOpen&) override { text = "LiftDoorOpen"; } void execute(const LiftSessionEnd&) override { text = "LiftSessionEnd"; } - void execute(const LiftMove&) override { text = "LiftMove"; } + 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"; } 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 ca6c78973..6a3f20681 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -696,6 +696,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() { 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 fa6573c85..1436abbe9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -216,12 +216,14 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftMove& move) final { + std::cout << "moving through " << move.lift_name() << std::endl; // TODO(MXG): We should probably keep track of what lift is being moved to // make sure we weren't given a broken nav graph _lifting_duration += move.duration(); _moving_lift = true; _continuous = true; + std::cout << " ^ executing move" << std::endl; } void execute(const LiftDoorOpen& open) final @@ -538,7 +540,7 @@ class LiftFinder : public rmf_traffic::agv::Graph::Lane::Executor // Do nothing } std::string current_name; - bool still_using; + bool still_using = false; void execute(const Dock& dock) final {} void execute(const Wait&) final {} @@ -546,6 +548,8 @@ class LiftFinder : public rmf_traffic::agv::Graph::Lane::Executor 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; } @@ -1014,6 +1018,7 @@ 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(); @@ -1143,31 +1148,49 @@ std::optional ExecutePlan::make( bool continuous = true; if (it->event()) { + std::cout << __LINE__ << ": t=" << rmf_traffic::time::to_seconds(it->time() - t0) << std::endl; + agv::EventPrinter printer; + it->event()->execute(printer); + std::cout << " ^ " << printer.text << " | " << it->event() << std::endl; + EventPhaseFactory factory( context, legacy_phases, *it, next_waypoint, plan_id, make_current_mutex_groups, get_new_mutex_groups, continuous); + + std::cout << __LINE__ << ": t=" << rmf_traffic::time::to_seconds(it->time() - t0) << std::endl; + it->event()->execute(printer); + std::cout << " ^ " << printer.text << " | " << it->event() << std::endl; + it->event()->execute(factory); + std::cout << __LINE__ << " finished executing" << std::endl; while (factory.moving_lift()) { 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 (!it->event()) { const double dist = (it->position().block<2, 1>(0, 0) - last_it->position().block<2, 1>(0, 0)).norm(); - if (dist < 0.5) + if (dist > 0.5) { // We'll assume that this is just a misalignment in the maps - 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."); } - 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; } factory.initial_waypoint = *it; 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 14b67e7bd..a3506a52d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -243,6 +243,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(); From 099cfe2944bd7540105d42a57710aa080c6862b6 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 27 Nov 2023 13:06:53 +0000 Subject: [PATCH 78/89] Add name to Destination API Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 4 ++ .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 54 +++++++++++++++++++ .../agv/internal_EasyFullControl.hpp | 1 + .../rmf_fleet_adapter/events/ExecutePlan.cpp | 30 +++++++++++ rmf_fleet_adapter_python/src/adapter.cpp | 1 + 5 files changed, 90 insertions(+) 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 43e12a0c2..42782e55b 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -438,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; 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 e24e6c53f..59a69473d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -738,6 +738,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 { @@ -1334,11 +1340,37 @@ void EasyCommandHandle::follow_new_path( } } + std::string name; + if (wp1.graph_index().has_value()) + { + const auto i1 = *wp1.graph_index(); + if (const auto* n = graph.get_waypoint(i1).name()) + { + name = *n; + } + else + { + const auto& stack_it = nav_params->stacked_vertices.find(i1); + if (stack_it != nav_params->stacked_vertices.end()) + { + for (const auto& v : *stack_it->second) + { + if (const auto* n = graph.get_waypoint(v).name()) + { + name = *n; + 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(), + name, speed_limit, in_lift); @@ -1542,10 +1574,32 @@ void EasyCommandHandle::dock( const auto command_position = to_robot_coordinates( wp1.get_map_name(), position); + std::string name; + if (const auto* n = wp1.name()) + { + name = *n; + } + else + { + const auto& stack_it = nav_params->stacked_vertices.find(i1); + if (stack_it != nav_params->stacked_vertices.end()) + { + for (const auto& v : *stack_it->second) + { + if (const auto* n = graph.get_waypoint(v).name()) + { + name = *n; + break; + } + } + } + } + auto destination = EasyFullControl::Destination::Implementation::make( wp1.get_map_name(), command_position, i1, + name, lane.properties().speed_limit(), wp1.in_lift(), dock_name_); 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 038b35093..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 @@ -76,6 +76,7 @@ class EasyFullControl::Destination::Implementation 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; 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 1436abbe9..e3be2c30e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -228,11 +228,41 @@ 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 (i1.has_value()) + { + if (const auto* n = graph.get_waypoint(*i1).name()) + { + name = *n; + } + else + { + if (const auto nav_params = _context->nav_params()) + { + const auto& stack_it = nav_params->stacked_vertices.find(*i1); + if (stack_it != nav_params->stacked_vertices.end()) + { + for (const auto& v : *stack_it->second) + { + if (const auto* n = graph.get_waypoint(v).name()) + { + name = *n; + break; + } + } + } + } + } + } + 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); diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 2060e2622..99c028e5b 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -858,6 +858,7 @@ 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("inside_lift", &agv::EasyFullControl::Destination::inside_lift); From 3b9d37fdb93a5dd2563f429018af2d3022737aad Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 27 Nov 2023 13:31:45 +0000 Subject: [PATCH 79/89] Make charger configuration optional Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 146 +++++++++--------- 1 file changed, 76 insertions(+), 70 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 59a69473d..3a682723c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -2346,7 +2346,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 " @@ -2354,60 +2354,60 @@ EasyFullControl::FleetConfiguration::from_config_files( return std::nullopt; } - std::string charger; - const YAML::Node& charger_yaml = robot_config_yaml["charger"]; - if (charger_yaml) + if (robot_config_yaml.IsMap()) { - charger = charger_yaml.as(); - } - else - { - 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::string charger; + const YAML::Node& charger_yaml = robot_config_yaml["charger"]; + if (charger_yaml) + { + charger = 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) + auto config = RobotConfiguration({std::move(charger)}, + responsive_wait, + max_merge_waypoint_distance, + max_merge_lane_distance, + min_lane_length); + known_robot_configurations.insert_or_assign(robot_name, config); + } + else { - min_lane_length = min_lane_length_yaml.as(); + auto config = RobotConfiguration({}); + known_robot_configurations.insert_or_assign(robot_name, config); } - - auto config = RobotConfiguration({std::move(charger)}, - responsive_wait, - max_merge_waypoint_distance, - max_merge_lane_distance, - min_lane_length); - known_robot_configurations.insert_or_assign(robot_name, config); } } } @@ -2906,36 +2906,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())); @@ -3067,7 +3070,10 @@ auto EasyFullControl::add_robot( .updater->handle = handle; handle->set_action_executor(action_executor); context->set_localization(localization); - handle->set_charger_waypoint(charger_index); + if (charger_index.has_value()) + { + handle->set_charger_waypoint(*charger_index); + } handle->enable_responsive_wait(enable_responsive_wait); RCLCPP_INFO( From c6f6ae1589ed3afdaf202ea207e745d04d53bf17 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 28 Nov 2023 03:28:21 +0000 Subject: [PATCH 80/89] Refactor vertex name search Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 50 +------------------ .../rmf_fleet_adapter/agv/RobotContext.cpp | 31 ++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 4 ++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 28 +++-------- 4 files changed, 44 insertions(+), 69 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 3a682723c..fbfde4a0a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1340,37 +1340,12 @@ void EasyCommandHandle::follow_new_path( } } - std::string name; - if (wp1.graph_index().has_value()) - { - const auto i1 = *wp1.graph_index(); - if (const auto* n = graph.get_waypoint(i1).name()) - { - name = *n; - } - else - { - const auto& stack_it = nav_params->stacked_vertices.find(i1); - if (stack_it != nav_params->stacked_vertices.end()) - { - for (const auto& v : *stack_it->second) - { - if (const auto* n = graph.get_waypoint(v).name()) - { - name = *n; - 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(), - name, + nav_params->get_vertex_name(graph, wp1.graph_index()), speed_limit, in_lift); @@ -1574,32 +1549,11 @@ void EasyCommandHandle::dock( const auto command_position = to_robot_coordinates( wp1.get_map_name(), position); - std::string name; - if (const auto* n = wp1.name()) - { - name = *n; - } - else - { - const auto& stack_it = nav_params->stacked_vertices.find(i1); - if (stack_it != nav_params->stacked_vertices.end()) - { - for (const auto& v : *stack_it->second) - { - if (const auto* n = graph.get_waypoint(v).name()) - { - name = *n; - break; - } - } - } - } - auto destination = EasyFullControl::Destination::Implementation::make( wp1.get_map_name(), command_position, i1, - name, + nav_params->get_vertex_name(graph, i1), lane.properties().speed_limit(), wp1.in_lift(), dock_name_); 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 57b440c05..f2062d015 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -129,6 +129,37 @@ void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) } } +//============================================================================== +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, 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 f8c14696a..dca57c2a6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -287,6 +287,10 @@ struct NavParams 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; 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 e3be2c30e..31bc3bb8e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -230,31 +230,17 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor { std::string name; const auto& graph = _context->navigation_graph(); - const auto i1= initial_waypoint.graph_index(); - if (i1.has_value()) + const auto i1 = initial_waypoint.graph_index(); + if (const auto nav_params = _context->nav_params()) { - if (const auto* n = graph.get_waypoint(*i1).name()) + 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; } - else - { - if (const auto nav_params = _context->nav_params()) - { - const auto& stack_it = nav_params->stacked_vertices.find(*i1); - if (stack_it != nav_params->stacked_vertices.end()) - { - for (const auto& v : *stack_it->second) - { - if (const auto* n = graph.get_waypoint(v).name()) - { - name = *n; - break; - } - } - } - } - } } const auto node = _context->node(); From 7b242132d8f275241810bb9393fdf80eb114b6ef Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 28 Nov 2023 04:41:29 +0000 Subject: [PATCH 81/89] Fix parsing of charger config Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/EasyFullControl.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 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 fbfde4a0a..ebdf2a9eb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -2310,11 +2310,11 @@ EasyFullControl::FleetConfiguration::from_config_files( if (robot_config_yaml.IsMap()) { - std::string charger; + std::vector chargers; const YAML::Node& charger_yaml = robot_config_yaml["charger"]; if (charger_yaml) { - charger = charger_yaml.as(); + chargers.push_back(charger_yaml.as()); } const YAML::Node& responsive_wait_yaml = @@ -2350,7 +2350,8 @@ EasyFullControl::FleetConfiguration::from_config_files( 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, From c28939f4c3c4ddb9e80ee5bf33d23af66a5f61ad Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 28 Nov 2023 05:15:27 +0000 Subject: [PATCH 82/89] Ignore empty-string dock names Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 45 ++++++++++--------- 1 file changed, 24 insertions(+), 21 deletions(-) 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 b04d0da1c..1c285c711 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 @@ -366,32 +366,35 @@ 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()); - 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()) + const rmf_traffic::Duration duration = std::chrono::seconds(5); + if (entry_event) { - dock_wp.set_in_lift(lift); - } + // 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(); + // First lane from start -> dock, second lane from dock -> end + begin = dock_wp.index(); - vnum_temp++; + 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( From 4143e29c668eb55fa97cdcaf7b1f3dac686f19c9 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 28 Nov 2023 16:00:00 +0000 Subject: [PATCH 83/89] Set a schedule horizon when lift sessions begin Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 95 ++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 19 ++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 129 +++++++++----- .../events/LockMutexGroup.cpp | 59 +------ .../rmf_fleet_adapter/phases/RequestLift.cpp | 165 +++++++++++------- .../rmf_fleet_adapter/phases/RequestLift.hpp | 37 ++-- .../test/phases/test_RequestLift.cpp | 9 +- 7 files changed, 330 insertions(+), 183 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 f2062d015..d85623d14 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1071,6 +1071,7 @@ std::shared_ptr RobotContext::set_lift_destination( std::string destination_floor, bool requested_from_inside) { + _lift_arrived = false; _lift_destination = std::make_shared( LiftDestination{ std::move(lift_name), @@ -1096,6 +1097,7 @@ void RobotContext::release_lift() _lift_destination = nullptr; _initial_time_idle_outside_lift = std::nullopt; _lift_stubbornness = nullptr; + _lift_arrived = false; } //============================================================================== @@ -1190,6 +1192,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) { @@ -1307,6 +1399,9 @@ void RobotContext::_check_lift_state( // Be a stubborn negotiator while using the lift _lift_stubbornness = be_stubborn(); } + + _lift_arrived = + _lift_destination->destination_floor == state.current_floor; } } 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 dca57c2a6..2205282c4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -335,6 +335,13 @@ 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; + } }; //============================================================================== @@ -640,6 +647,17 @@ class RobotContext /// 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); @@ -800,6 +818,7 @@ class RobotContext 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); 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 31bc3bb8e..81d02761d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -108,6 +108,53 @@ MakeStandby make_wait_for_traffic( }; } +//============================================================================== +void truncate_arrival( + rmf_traffic::schedule::Itinerary& previous_itinerary, + const rmf_traffic::agv::Plan::Waypoint& wp, + std::stringstream& ss) +{ + 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(); + ss << "\n -- erasing from checkpoint " << c.checkpoint_id + << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; + + ss << " t.size() before: " << t.size(); + t.erase(t.begin() + (int)c.checkpoint_id, t.end()); + ss << " vs after: " << t.size(); + + if (t.size() > 0) + { + ss << " ending at <" << t.back().position().transpose() << ">"; + } + } + + for (std::size_t i=0; i < previous_itinerary.size(); ++i) + { + const auto& t = previous_itinerary.at(i).trajectory(); + if (t.size() < 2) + { + ss << "\n -- excluding from " << i; + // 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 { @@ -126,6 +173,8 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor 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) : initial_waypoint(initial_waypoint_), next_waypoint(std::move(next_waypoint_)), @@ -135,6 +184,8 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _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 @@ -199,6 +250,16 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftSessionBegin& open) final { + std::stringstream ss; + ss << "truncating for LiftSessionBegin at [" << open.lift_name() << "] for [" + << _context->requester_id() << "]"; + truncate_arrival(*_previous_itinerary, initial_waypoint, ss); + + std::cout << ss.str() << std::endl; + + _previous_itinerary = + std::make_shared(_full_itinerary); + assert(!_moving_lift); const auto node = _context->node(); _phases.emplace_back( @@ -206,9 +267,14 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, open.lift_name(), open.floor_name(), - _event_start_time, - phases::RequestLift::Located::Outside, - _plan_id), + 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; @@ -258,10 +324,12 @@ 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, - _plan_id, - localize), + 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; @@ -302,12 +370,13 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor 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 { @@ -769,7 +838,8 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( - context, lift->name(), map, t0, side, plan_id), + context, lift->name(), map, + phases::RequestLift::Data{t0, side, plan_id}), t0, rmf_traffic::Dependencies(), std::nullopt); } } @@ -880,8 +950,6 @@ std::optional ExecutePlan::make( waypoints.size()); } } - - std::size_t first_excluded_route = 0; std::stringstream ss; if (current_mutex_groups.has_value()) { @@ -895,48 +963,12 @@ std::optional ExecutePlan::make( ss << "truncating to lock " << all_str(new_mutex_groups) << " for [" << context->requester_id() << "]"; } - 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(); - ss << "\n -- erasing from checkpoint " << c.checkpoint_id - << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; - ss << " t.size() before: " << t.size(); - t.erase(t.begin() + (int)c.checkpoint_id, t.end()); - ss << " vs after: " << t.size(); - - if (t.size() > 0) - { - ss << " ending at <" << t.back().position().transpose() << ">"; - } - } - - for (std::size_t i=0; i < previous_itinerary->size(); ++i) - { - const auto& t = previous_itinerary->at(i).trajectory(); - if (t.size() < 2) - { - ss << "\n -- excluding from " << i; - // 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); - } - } + truncate_arrival(*previous_itinerary, wp, ss); auto expected_waypoints = waypoints; expected_waypoints.insert(expected_waypoints.begin(), wp); - previous_itinerary->erase( - previous_itinerary->begin()+first_excluded_route, - previous_itinerary->end()); - std::cout << ss.str() << std::endl; auto next_itinerary = std::make_shared< @@ -1172,6 +1204,7 @@ std::optional ExecutePlan::make( EventPhaseFactory factory( context, legacy_phases, *it, next_waypoint, plan_id, make_current_mutex_groups, get_new_mutex_groups, + previous_itinerary, full_itinerary, continuous); std::cout << __LINE__ << ": t=" << rmf_traffic::time::to_seconds(it->time() - t0) << std::endl; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index a78faf5ba..c5b610f30 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -185,13 +185,13 @@ void LockMutexGroup::Active::_initialize() } *_data.plan_id += 1; - rmf_traffic::Trajectory hold; - const auto zero = Eigen::Vector3d::Zero(); - const auto wait = std::chrono::seconds(5); - hold.insert(_data.hold_time, _data.hold_position, zero); - hold.insert(_data.hold_time + wait, _data.hold_position, zero); + _context->schedule_hold( + _data.plan_id, + _data.hold_time, + std::chrono::seconds(5), + _data.hold_position, + _data.hold_map); _stubborn = _context->be_stubborn(); - _schedule({rmf_traffic::Route(_data.hold_map, std::move(hold))}); _state->update_log().info( "Waiting to lock mutex group " + _data.all_groups_str()); @@ -340,52 +340,7 @@ void LockMutexGroup::Active::_schedule( { std::cout << " --- [" << _context->requester_id() << "] resuming with " << itinerary.size() << " routes" << std::endl; - - bool scheduled = false; - std::size_t attempts = 0; - while (!scheduled) - { - if (++attempts > 5) - { - std::stringstream ss_sizes; - for (const auto& r : itinerary) - { - ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; - } - - RCLCPP_ERROR( - _context->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.", - itinerary.size(), - ss_sizes.str().c_str(), - _context->requester_id().c_str(), - *_data.plan_id); - break; - } - - scheduled = _context->itinerary().set(*_data.plan_id, itinerary); - - if (!scheduled) - { - *_data.plan_id = _context->itinerary().assign_plan_id(); - if (attempts > 1) - { - RCLCPP_ERROR( - _context->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.", - *_data.plan_id, - _context->itinerary().current_plan_id(), - _context->requester_id().c_str()); - } - } - } + _context->schedule_itinerary(_data.plan_id, std::move(itinerary)); } //============================================================================== 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 a3506a52d..abffa8372 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -29,21 +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, - rmf_traffic::PlanId plan_id, - std::optional localize) + Data data) { auto inst = std::shared_ptr( new ActivePhase( std::move(context), std::move(lift_name), std::move(destination), - std::move(expected_finish), - located, - plan_id, - std::move(localize) + std::move(data) )); + inst->_init_obs(); return inst; } @@ -85,17 +80,11 @@ RequestLift::ActivePhase::ActivePhase( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located, - rmf_traffic::PlanId plan_id, - std::optional localize) + Data data) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), - _expected_finish(std::move(expected_finish)), - _located(located), - _plan_id(plan_id), - _localize_after(std::move(localize)) + _data(std::move(data)) { std::ostringstream oss; oss << "Requesting lift [" << lift_name << "] to [" << destination << "]"; @@ -108,6 +97,47 @@ 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( @@ -130,13 +160,13 @@ 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 delay = me->_context->now() - me->_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, - plan_id = me->_plan_id, + plan_id = *me->_data.plan_id, delay ](const auto&) { @@ -177,29 +207,35 @@ void RequestLift::ActivePhase::_init_obs() if (!me) return; - if (me->_localize_after.has_value()) + if (me->_data.localize_after.has_value()) { auto finish = [s, worker = me->_context->worker(), weak]() { worker.schedule([s, weak](const auto&) { if (const auto me = weak.lock()) - me->_finish(); + { + if (!me->_finish()) + { + return; + } + } s.on_completed(); }); }; + auto cmd = agv::EasyFullControl ::CommandExecution::Implementation::make_hold( me->_context, - me->_expected_finish, - me->_plan_id, + me->_data.expected_finish, + *me->_data.plan_id, std::move(finish)); - agv::Destination::Implementation::get(*me->_localize_after) + agv::Destination::Implementation::get(*me->_data.localize_after) .position = me->_context->position(); - if (me->_context->localize(*me->_localize_after, std::move(cmd))) + 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), @@ -217,15 +253,15 @@ void RequestLift::ActivePhase::_init_obs() "process is finished.", me->_context->requester_id().c_str()); - me->_finish(); - s.on_completed(); + if (me->_finish()) + s.on_completed(); }); return; } } - me->_finish(); - s.on_completed(); + if (me->_finish()) + s.on_completed(); })); } @@ -304,7 +340,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( @@ -356,19 +392,54 @@ void RequestLift::ActivePhase::_do_publish() if (!_destination_handle) { _destination_handle = _context->set_lift_destination( - _lift_name, _destination, _located == Located::Inside); + _lift_name, _destination, _data.located == Located::Inside); } } //============================================================================== -void RequestLift::ActivePhase::_finish() +bool RequestLift::ActivePhase::_finish() { - if (_located == Located::Outside) + // 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; } + + return true; } //============================================================================== @@ -376,17 +447,11 @@ RequestLift::PendingPhase::PendingPhase( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located, - PlanIdPtr plan_id, - std::optional localize) + Data data) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), - _expected_finish(std::move(expected_finish)), - _located(located), - _plan_id(plan_id), - _localize_after(std::move(localize)) + _data(std::move(data)) { std::ostringstream oss; oss << "Requesting lift \"" << lift_name << "\" to \"" << destination << "\""; @@ -397,29 +462,11 @@ RequestLift::PendingPhase::PendingPhase( //============================================================================== std::shared_ptr RequestLift::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 RequestLift action for robot [%s]. This is " - "a critical internal error, please report this bug to the RMF " - "maintainers.", - _context->requester_id().c_str()); - } - return ActivePhase::make( _context, _lift_name, _destination, - _expected_finish, - _located, - plan_id, - _localize_after); + _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 d05249aef..45222d240 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,16 @@ 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,10 +53,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located, - rmf_traffic::PlanId plan_id, - std::optional localize); + Data data); const rxcpp::observable& observe() const override; @@ -63,18 +70,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_traffic::PlanId _plan_id; - std::optional _localize_after; rmf_rxcpp::subscription_guard _reset_session_subscription; std::shared_ptr _destination_handle; + bool _finished = false; struct WatchdogInfo { @@ -90,10 +95,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located, - rmf_traffic::PlanId plan_id, - std::optional localize); + Data data); void _init_obs(); @@ -101,7 +103,7 @@ struct RequestLift const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state); void _do_publish(); - void _finish(); + bool _finish(); }; class PendingPhase : public LegacyTask::PendingPhase @@ -112,10 +114,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located, - PlanIdPtr plan_id, - std::optional localize = std::nullopt); + Data data); std::shared_ptr begin() override; @@ -133,10 +132,8 @@ struct RequestLift std::string _lift_name; std::string _destination; rmf_traffic::Time _expected_finish; - Located _located; - PlanIdPtr _plan_id; - std::optional _localize_after; std::string _description; + Data _data; }; }; diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index a3fc043c7..5ce6a23bf 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -70,10 +70,11 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") context, lift_name, destination, - context->now() + std::chrono::seconds(5), - RequestLift::Located::Outside, - std::make_shared(0) - ); + 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") From 65e45357d581c4276852b31e71d13b6f2ad867f8 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 29 Nov 2023 08:56:09 +0800 Subject: [PATCH 84/89] Publish timestamp from lift supervisor Signed-off-by: Xiyu Oh --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 190f80e70..24e81369f 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -58,8 +58,8 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) RCLCPP_INFO( this->get_logger(), - "[%s] Received adapter lift request to [%s]", - msg->session_id.c_str(), msg->destination_floor.c_str() + "[%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; @@ -72,6 +72,7 @@ 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(), @@ -103,6 +104,7 @@ 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->request_time = this->now(); _lift_request_pub->publish(*lift_request); RCLCPP_INFO( this->get_logger(), @@ -118,6 +120,7 @@ void Node::_lift_state_update(LiftState::UniquePtr msg) 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); } From fb47ab0f31d7f96a5a8095d15125f39de37c6ff3 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 29 Nov 2023 14:29:22 +0000 Subject: [PATCH 85/89] Remove unnecessary debug Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 131 -------------- .../rmf_fleet_adapter/agv/RobotContext.cpp | 49 ----- .../rmf_fleet_adapter/agv/RobotContext.hpp | 47 ++++- .../agv/internal_FleetUpdateHandle.hpp | 2 - .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 19 -- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 170 +----------------- .../rmf_fleet_adapter/events/GoToPlace.cpp | 20 +-- .../events/LockMutexGroup.cpp | 5 - .../events/WaitForTraffic.cpp | 22 --- .../rmf_fleet_adapter/phases/DockRobot.hpp | 7 - .../rmf_fleet_adapter/phases/MoveRobot.cpp | 8 - .../rmf_fleet_adapter/phases/MoveRobot.hpp | 13 -- .../rmf_fleet_adapter/phases/RequestLift.cpp | 2 +- .../rmf_fleet_adapter/project_itinerary.cpp | 12 +- .../rmf_fleet_adapter/project_itinerary.hpp | 3 +- .../schedule/MirrorManager.cpp | 20 --- .../src/rmf_traffic_ros2/schedule/Node.cpp | 27 --- .../schedule/internal_Node.hpp | 2 - 18 files changed, 61 insertions(+), 498 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 ebdf2a9eb..1713900b1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -452,7 +452,6 @@ class EasyFullControl::CommandExecution::Implementation::Data if (starts.empty()) { - std::cout << __LINE__ << " " << context->requester_id() << " compute_plan_starts" << std::endl; starts = nav_params->compute_plan_starts(graph, map, location, now); } @@ -780,10 +779,8 @@ struct ProgressTracker : std::enable_shared_from_this void next() { - std::cout << " >>> " << __LINE__ << " rq: " << reverse_queue.size() << std::endl; if (reverse_queue.empty()) { - std::cout << " >>> " << __LINE__ << " trigger finish" << std::endl; current_identifier = nullptr; finished.trigger(); return; @@ -795,23 +792,18 @@ struct ProgressTracker : std::enable_shared_from_this ::get(current_activity).identifier; auto& current_activity_impl = EasyFullControl::CommandExecution::Implementation::get(current_activity); - std::cout << " >>> " << __LINE__ << " make next finisher" << std::endl; current_activity_impl.finisher = [w_progress = weak_from_this()]() { - std::cout << " >>> " << __LINE__ << " lock progress" << std::endl; if (const auto progress = w_progress.lock()) { - std::cout << " >>> " << __LINE__ << " trigger next" << std::endl; progress->next(); } }; - std::cout << " >>> " << __LINE__ << " prepare begin" << std::endl; const auto begin = current_activity_impl.begin; if (begin) { - std::cout << " >>> " << __LINE__ << " do begin" << std::endl; begin(std::move(current_activity)); } } @@ -975,39 +967,6 @@ void EasyCommandHandle::follow_new_path( } const auto& graph = planner->get_configuration().graph(); std::vector waypoints = cmd_waypoints; - // std::size_t i_flatten = 1; - // while (i_flatten < waypoints.size()) - // { - // const auto& wp0 = waypoints[i_flatten-1]; - // const auto& wp1 = waypoints[i_flatten]; - // if (!wp0.graph_index().has_value() && !wp1.graph_index().has_value()) - // { - // waypoints.erase(waypoints.begin() + i_flatten); - // continue; - // } - - // if (!wp0.graph_index().has_value() || !wp1.graph_index().has_value()) - // { - // ++i_flatten; - // continue; - // } - - // const auto i0 = *wp0.graph_index(); - // const auto i1 = *wp1.graph_index(); - // if (nav_params->in_same_stack(i0, i1)) - // { - // const auto yaw_diff = rmf_utils::wrap_to_pi( - // wp0.position()[2] - wp1.position()[2]); - // if (std::abs(yaw_diff) < 5*M_PI/180.0) - // { - // // These waypoints are redundant for moving so let's filter one out - // waypoints.erase(waypoints.begin() + i_flatten); - // continue; - // } - // } - - // ++i_flatten; - // } if (waypoints.size() < 2) { @@ -1055,86 +1014,15 @@ void EasyCommandHandle::follow_new_path( std::vector queue; const auto& current_location = context->location(); - auto print_location = [&](const rmf_traffic::agv::Plan::Start& l) - { - std::stringstream ss; - if (l.lane().has_value()) - { - ss << "lane[" << *l.lane() << "] "; - EventPrinter printer; - const auto& lane = graph.get_lane(*l.lane()); - if (lane.entry().event()) - { - ss << " [entry "; - lane.entry().event()->execute(printer); - ss << printer.text << "] "; - } - const auto& i_wp0 = lane.entry().waypoint_index(); - const auto& wp0 = graph.get_waypoint(i_wp0); - const auto& i_wp1 = lane.exit().waypoint_index(); - const auto& wp1 = graph.get_waypoint(i_wp1); - ss << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() - << "] -> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - if (lane.exit().event()) - { - ss << "[exit "; - lane.exit().event()->execute(printer); - ss << printer.text << "]"; - } - ss << " | "; - } - ss << l.waypoint(); - if (l.location().has_value()) - { - ss << " | " << l.location()->transpose(); - } - return ss.str(); - }; - - std::stringstream ss; - ss << context->requester_id() << " Locations: " << current_location.size(); - for (const auto& l : current_location) - { - ss << "\n -- " << print_location(l); - } - ss << "\nNum waypoints: " << waypoints.size(); - for (const rmf_traffic::agv::Plan::Waypoint& wp : waypoints) - { - ss << "\n -- " << print_plan_waypoint(wp, graph, waypoints.front().time()); - // if (wp.graph_index().has_value()) - // { - // ss << " index " << *wp.graph_index(); - // const auto& gwp = graph.get_waypoint(*wp.graph_index()); - // if (!gwp.in_mutex_group().empty()) - // { - // ss << " *" << gwp.in_mutex_group(); - // } - - // ss << " [" << gwp.get_map_name() << "]"; - // } - // ss << " " << wp.position().transpose(); - - // ss << " approach lanes:"; - // for (const auto& l : wp.approach_lanes()) - // { - // ss << " " << l; - // } - } - std::cout << ss.str() << std::endl; - bool found_connection = false; std::size_t i0 = 0; - ss = std::stringstream(); - ss << "Finding connection for " << context->requester_id(); for (std::size_t i = 0; i < waypoints.size(); ++i) { const auto& wp = waypoints[i]; if (wp.graph_index().has_value()) { - ss << " \n -- " << wp.graph_index().value() << "\n"; for (const auto& l : current_location) { - ss << "location: " << l.waypoint() << "\n"; if (nav_params->in_same_stack(*wp.graph_index(), l.waypoint()) && !l.lane().has_value()) { if (l.location().has_value()) @@ -1148,7 +1036,6 @@ void EasyCommandHandle::follow_new_path( { const double dist = (*l.location() - wp.position().block<2, 1>(0, 0)).norm(); - ss << dist << " vs " << nav_params->max_merge_lane_distance << "\n"; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1162,15 +1049,10 @@ void EasyCommandHandle::follow_new_path( i0 = i; } } - else - { - ss << "not the same stack: " << l.waypoint() << " vs " << wp.graph_index().value() << "\n"; - } } } else { - ss << "\n -- " << wp.position().transpose() << "\n"; for (const auto& l : current_location) { Eigen::Vector2d p_l; @@ -1183,7 +1065,6 @@ void EasyCommandHandle::follow_new_path( p_l = graph.get_waypoint(l.waypoint()).get_location(); } const double dist = (wp.position().block<2, 1>(0, 0) - p_l).norm(); - ss << dist << " vs " << nav_params->max_merge_lane_distance << "\n"; if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1194,16 +1075,12 @@ void EasyCommandHandle::follow_new_path( if (i > 0) { - std::stringstream ss; - ss << "approach lanes:"; for (const auto lane : wp.approach_lanes()) { - ss << " " << lane; for (const auto& l : current_location) { if (l.lane().has_value()) { - ss << " [vs " << *l.lane() << "]"; if (lane == *l.lane()) { found_connection = true; @@ -1212,12 +1089,9 @@ void EasyCommandHandle::follow_new_path( } } } - std::cout << ss.str() << std::endl; } } - std::cout << ss.str() << std::endl; - if (!found_connection) { // The robot has drifted away from the starting point since the plan started @@ -1240,8 +1114,6 @@ void EasyCommandHandle::follow_new_path( i0 = waypoints.size() - 2; } - std::stringstream wss; - wss << " ----------- " << context->requester_id() << " Moving through:"; std::size_t i1 = i0 + 1; while (i1 < waypoints.size()) { @@ -1350,7 +1222,6 @@ void EasyCommandHandle::follow_new_path( in_lift); const auto target_p = waypoints.at(target_index).position(); - wss << "\n -- " << target_p.transpose(); queue.push_back( EasyFullControl::CommandExecution::Implementation::make( context, @@ -1380,8 +1251,6 @@ void EasyCommandHandle::follow_new_path( i1 = i0 + 1; } - std::cout << wss.str() << std::endl; - this->current_progress = ProgressTracker::make( queue, path_finished_callback_); 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 d85623d14..861374dc8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -219,7 +219,6 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( { if (graph.lane_from(v, waypoint)) { - // std::cout << "descending " << v << " -> " << waypoint << std::endl; waypoint = v; can_descend = true; break; @@ -234,7 +233,6 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( // Transfer the location estimate over to the waypoint that's at the bottom // of the vertex stack. - // std::cout << "Descended vertex stack to " << waypoint << std::endl; if (waypoint != original_waypoint) { bool can_merge = true; @@ -426,53 +424,6 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) _location = std::move(location_); filter_closed_lanes(); - // std::stringstream ss; - // ss << requester_id() << " locations"; - // for (const auto& l : _location) - // { - // ss << " | "; - // if (l.lane().has_value()) - // { - // ss << "lane[" << *l.lane() << "] "; - // EventPrinter printer; - // const auto& lane = graph.get_lane(*l.lane()); - // if (lane.entry().event()) - // { - // ss << " [entry "; - // lane.entry().event()->execute(printer); - // ss << "] "; - // } - // const auto& i_wp0 = lane.entry().waypoint_index(); - // const auto& wp0 = graph.get_waypoint(i_wp0); - // const auto& i_wp1 = lane.exit().waypoint_index(); - // const auto& wp1 = graph.get_waypoint(i_wp1); - // ss << i_wp0 << " [" << wp0.get_map_name() << ":" << wp0.get_location().transpose() << "] "; - // if (const auto lift = wp0.in_lift()) - // ss << "{" << lift->name() << "} "; - // ss << "-> " << i_wp1 << " [" << wp1.get_map_name() << ":" << wp1.get_location().transpose() << "] "; - // if (const auto lift = wp1.in_lift()) - // ss << "{" << lift->name() << "} "; - - // if (lane.exit().event()) - // { - // ss << "[exit "; - // lane.exit().event()->execute(printer); - // ss << "]"; - // } - // ss << " | "; - // } - // ss << l.waypoint(); - // const auto& wp = graph.get_waypoint(l.waypoint()); - // if (const auto lift = wp.in_lift()) - // { - // ss << " {" << lift->name() << "}"; - // } - // if (l.location().has_value()) - // { - // ss << " | " << l.location()->transpose(); - // } - // } - if (_location.empty()) { if (debug_positions) 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 2205282c4..eaf7a0906 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -70,14 +70,45 @@ class EventPrinter : public rmf_traffic::agv::Graph::Lane::Executor // Do nothing } - void execute(const DoorOpen&) override { text = "DoorOpen"; } - void execute(const DoorClose&) override { text = "DoorClose"; } - void execute(const LiftSessionBegin&) override { text = "LiftSessionBegin"; } - void execute(const LiftDoorOpen&) override { text = "LiftDoorOpen"; } - void execute(const LiftSessionEnd&) override { text = "LiftSessionEnd"; } - 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"; } + 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; }; 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 04e9be790..49a13f533 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 @@ -357,8 +357,6 @@ class FleetUpdateHandle::Implementation std::shared_ptr calculate_bid; rmf_rxcpp::subscription_guard calculate_bid_subscription; - - template static std::shared_ptr make(Args&&... args) { 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 1c285c711..fe14ba0d5 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 @@ -480,25 +480,6 @@ rmf_traffic::agv::Graph parse_graph( } } - std::cout << " ========= Mutex groups:" << std::endl; - for (std::size_t i=0; i < graph.num_waypoints(); ++i) - { - const auto& m = graph.get_waypoint(i).in_mutex_group(); - if (!m.empty()) - { - std::cout << " -- " << i << ": " << m << std::endl; - } - } - - for (std::size_t i=0; i < graph.num_lanes(); ++i) - { - const auto& m = graph.get_lane(i).properties().in_mutex_group(); - if (!m.empty()) - { - std::cout << " -- " << i << ": " << m << std::endl; - } - } - return graph; } 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 81d02761d..b17feb0f5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -111,8 +111,7 @@ MakeStandby make_wait_for_traffic( //============================================================================== void truncate_arrival( rmf_traffic::schedule::Itinerary& previous_itinerary, - const rmf_traffic::agv::Plan::Waypoint& wp, - std::stringstream& ss) + const rmf_traffic::agv::Plan::Waypoint& wp) { std::size_t first_excluded_route = 0; for (const auto& c : wp.arrival_checkpoints()) @@ -120,17 +119,8 @@ void truncate_arrival( first_excluded_route = std::max(first_excluded_route, c.route_id+1); auto& r = previous_itinerary.at(c.route_id); auto& t = r.trajectory(); - ss << "\n -- erasing from checkpoint " << c.checkpoint_id - << " <" << t.at(c.checkpoint_id).position().transpose() << ">"; - ss << " t.size() before: " << t.size(); t.erase(t.begin() + (int)c.checkpoint_id, t.end()); - ss << " vs after: " << t.size(); - - if (t.size() > 0) - { - ss << " ending at <" << t.back().position().transpose() << ">"; - } } for (std::size_t i=0; i < previous_itinerary.size(); ++i) @@ -138,7 +128,6 @@ void truncate_arrival( const auto& t = previous_itinerary.at(i).trajectory(); if (t.size() < 2) { - ss << "\n -- excluding from " << i; // 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 @@ -250,12 +239,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftSessionBegin& open) final { - std::stringstream ss; - ss << "truncating for LiftSessionBegin at [" << open.lift_name() << "] for [" - << _context->requester_id() << "]"; - truncate_arrival(*_previous_itinerary, initial_waypoint, ss); - - std::cout << ss.str() << std::endl; + truncate_arrival(*_previous_itinerary, initial_waypoint); _previous_itinerary = std::make_shared(_full_itinerary); @@ -282,14 +266,12 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftMove& move) final { - std::cout << "moving through " << move.lift_name() << std::endl; // TODO(MXG): We should probably keep track of what lift is being moved to // make sure we weren't given a broken nav graph _lifting_duration += move.duration(); _moving_lift = true; _continuous = true; - std::cout << " ^ executing move" << std::endl; } void execute(const LiftDoorOpen& open) final @@ -596,24 +578,6 @@ std::optional search_for_lift_group( } // anonymous namespace -class Printer : public rmf_traffic::agv::Graph::Lane::Executor -{ -public: - Printer() - { - // Do nothing - } - - void execute(const DoorOpen&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const DoorClose&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const LiftSessionBegin&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const LiftDoorOpen&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const LiftSessionEnd&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const LiftMove&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const Wait&) override { std::cout << "event " << __LINE__ << std::endl;; } - void execute(const Dock& dock) override { std::cout << "event " << __LINE__ << std::endl;; } -}; - //============================================================================== class LiftFinder : public rmf_traffic::agv::Graph::Lane::Executor { @@ -701,20 +665,6 @@ std::optional ExecutePlan::make( { if (plan.get_waypoints().empty()) return std::nullopt; - // std::cout << " --- plan --- " << std::endl; - // const auto t0 = plan.get_waypoints().front().time(); - // for (const rmf_traffic::agv::Plan::Waypoint& wp : plan.get_waypoints()) - // { - // std::cout << " -- t=" << rmf_traffic::time::to_seconds(wp.time() - t0) << " "; - // if (wp.graph_index().has_value()) - // std::cout << "index " << *wp.graph_index() << " "; - // std::cout << "[" << wp.position().transpose() << "]" << std::endl; - // if (wp.event()) - // { - // Printer printer; - // wp.event()->execute(printer); - // } - // } auto plan_id = std::make_shared(recommended_plan_id); const auto& graph = context->navigation_graph(); @@ -725,7 +675,6 @@ std::optional ExecutePlan::make( ->get_characteristic_length()/2.0; if (const auto* current_lift = context->current_lift_destination()) { - std::cout << " :::::::::: " << context->requester_id() << " STILL HAVE LIFT LOCKED: " << current_lift->lift_name << std::endl; LiftFinder finder(current_lift->lift_name); for (const auto& wp : plan.get_waypoints()) { @@ -753,24 +702,21 @@ std::optional ExecutePlan::make( } else { - std::cout << "KNOWN LIFTS:"; + std::stringstream ss; for (const auto& l : graph.all_known_lifts()) { - std::cout << "\n -- " << l->name(); + ss << "[" << l->name() << "]"; } - std::cout << std::endl; RCLCPP_ERROR( context->node()->get_logger(), - "Robot [%s] might be stuck with locking an unknown lift named [%s]", + "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()); + current_lift->lift_name.c_str(), + ss.str().c_str()); } } - else - { - std::cout << " ::::::::::::: " << context->requester_id() << " STILL USING THE LIFT: " << current_lift->lift_name << std::endl; - } } if (!plan.get_waypoints().front().graph_index().has_value()) @@ -779,7 +725,6 @@ std::optional ExecutePlan::make( plan.get_waypoints().front().position().block<2, 1>(0, 0); const auto t0 = plan.get_waypoints().front().time(); - std::cout << " === " << __LINE__ << " broken start" << std::endl; const auto first_graph_wp = [&]() -> std::optional { for (const auto& wp : plan.get_waypoints()) @@ -793,7 +738,6 @@ std::optional ExecutePlan::make( if (first_graph_wp.has_value()) { - std::cout << " === " << __LINE__ << " merging onto " << *first_graph_wp << std::endl; const Eigen::Vector2d p1 = graph.get_waypoint(*first_graph_wp).get_location(); @@ -896,19 +840,6 @@ std::optional ExecutePlan::make( std::vector waypoints = plan.get_waypoints(); - std::stringstream pss; - pss << "Plan waypoints"; - for (const auto& wp : waypoints) - { - pss << "\n -- " << agv::print_plan_waypoint(wp, graph, waypoints.front().time()); - } - pss << "\nItineraries:"; - for (const auto& r : full_itinerary) - { - pss << "\n -- " << r.map() << ":" << r.trajectory().size(); - } - std::cout << pss.str() << std::endl; - std::vector move_through; std::optional current_mutex_groups; std::unordered_set remaining_mutex_groups; @@ -950,27 +881,12 @@ std::optional ExecutePlan::make( waypoints.size()); } } - std::stringstream ss; - if (current_mutex_groups.has_value()) - { - ss << "truncating for switch from [" - << current_mutex_groups->all_groups_str() - << "] to [" << all_str(new_mutex_groups) << "] for [" - << context->requester_id() << "]"; - } - else - { - ss << "truncating to lock " << all_str(new_mutex_groups) << " for [" - << context->requester_id() << "]"; - } - truncate_arrival(*previous_itinerary, wp, ss); + truncate_arrival(*previous_itinerary, wp); auto expected_waypoints = waypoints; expected_waypoints.insert(expected_waypoints.begin(), wp); - std::cout << ss.str() << std::endl; - auto next_itinerary = std::make_shared< rmf_traffic::schedule::Itinerary>(full_itinerary); auto data = LockMutexGroup::Data{ @@ -992,63 +908,40 @@ std::optional ExecutePlan::make( const auto get_new_mutex_groups = [&]( const rmf_traffic::agv::Plan::Waypoint& wp) { - std::stringstream ss; std::unordered_set new_mutex_groups; - ss << "<" << wp.position().transpose() << "> "; if (wp.graph_index().has_value()) { - ss << "i_wp: " << *wp.graph_index() << " "; - if (const auto* name = graph.get_waypoint(*wp.graph_index()).name()) - { - ss << "name_wp: " << *name << " "; - } - const auto& group = graph.get_waypoint(*wp.graph_index()).in_mutex_group(); if (!group.empty()) { - ss << " [" << group << "]"; new_mutex_groups.insert(group); } } - ss << " lanes: "; for (const auto l : wp.approach_lanes()) { - ss << l << " "; const auto& lane = graph.get_lane(l); const auto& group = lane.properties().in_mutex_group(); if (!group.empty()) { - ss << "[" << group << "] "; new_mutex_groups.insert(group); break; } } - std::cout << " ===>>> " << __LINE__ << " " << all_str(new_mutex_groups) - << " | " << ss.str() << std::endl; - bool mutex_group_change = (!new_mutex_groups.empty() && remaining_mutex_groups.empty()); - std::cout << __LINE__ << " | " << !new_mutex_groups.empty() - << " " << !current_mutex_groups.has_value() << std::endl; - if (!mutex_group_change && !remaining_mutex_groups.empty()) { for (const auto& new_group : new_mutex_groups) { if (remaining_mutex_groups.count(new_group) == 0) { - std::cout << " === " << __LINE__ << " new group: " << new_group << std::endl; mutex_group_change = true; break; } - else - { - std::cout << " === " << __LINE__ << " old group: " << new_group << std::endl; - } } } @@ -1061,8 +954,6 @@ std::optional ExecutePlan::make( remaining_mutex_groups = new_mutex_groups; } - std::cout << " === " << __LINE__ << " | mutex_group_changed: " - << mutex_group_change << std::endl; return std::make_pair(mutex_group_change, new_mutex_groups); }; @@ -1074,23 +965,13 @@ std::optional ExecutePlan::make( for (; it != waypoints.end(); ++it) { const auto [mutex_group_change, new_mutex_groups] = get_new_mutex_groups(*it); - // std::cout << new_mutex_groups << " | " << current_mutex_groups.has_value(); - // if (current_mutex_groups.has_value()) - // std::cout << " | " << current_mutex_groups->all_groups_str(); - // std::cout << std::endl; - if (mutex_group_change) { if (move_through.size() > 1) { - std::cout << " >>> " << __LINE__ << std::endl; auto next_mutex_group = make_current_mutex_groups( new_mutex_groups, move_through.back()); - // if (current_mutex_groups.has_value()) - // std::cout << " === " << __LINE__ << ": " << current_mutex_groups->mutex_group << std::endl; - // else - // std::cout << " === " << __LINE__ << ": " << std::endl; legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), @@ -1101,8 +982,6 @@ std::optional ExecutePlan::make( move_through.clear(); // Repeat the last waypoint so that follow_new_path has continuity. move_through.push_back(last); - std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph); - waypoints.erase(waypoints.begin(), it); current_mutex_groups = next_mutex_group; @@ -1118,25 +997,19 @@ std::optional ExecutePlan::make( // through yet. Just set the current_mutex_groups from this point. if (move_through.empty()) { - std::cout << " >>> " << __LINE__ << std::endl; current_mutex_groups = make_current_mutex_groups( new_mutex_groups, *it); - std::cout << " === " << __LINE__ << ": " << current_mutex_groups->all_groups_str() << std::endl; } else { - std::cout << " >>> " << __LINE__ << std::endl; assert(move_through.size() == 1); current_mutex_groups = make_current_mutex_groups( new_mutex_groups, move_through.back()); - std::cout << " === " << __LINE__ << ": " << current_mutex_groups->all_groups_str() << std::endl; } } } move_through.push_back(*it); - std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph) << std::endl; - 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); @@ -1196,23 +1069,13 @@ std::optional ExecutePlan::make( bool continuous = true; if (it->event()) { - std::cout << __LINE__ << ": t=" << rmf_traffic::time::to_seconds(it->time() - t0) << std::endl; - agv::EventPrinter printer; - it->event()->execute(printer); - std::cout << " ^ " << printer.text << " | " << it->event() << std::endl; - EventPhaseFactory factory( context, legacy_phases, *it, next_waypoint, plan_id, make_current_mutex_groups, get_new_mutex_groups, previous_itinerary, full_itinerary, continuous); - std::cout << __LINE__ << ": t=" << rmf_traffic::time::to_seconds(it->time() - t0) << std::endl; - it->event()->execute(printer); - std::cout << " ^ " << printer.text << " | " << it->event() << std::endl; - it->event()->execute(factory); - std::cout << __LINE__ << " finished executing" << std::endl; while (factory.moving_lift()) { const auto last_it = it; @@ -1252,7 +1115,6 @@ std::optional ExecutePlan::make( // Have the next sequence of waypoints begin with the event waypoint // of this sequence. move_through.push_back(*it); - std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph) << std::endl; } waypoints.erase(waypoints.begin(), it+1); @@ -1277,7 +1139,6 @@ std::optional ExecutePlan::make( // Have the next sequence of waypoints begin with this one. move_through.clear(); move_through.push_back(*it); - std::cout << " >> push " << __LINE__ << ": " << print_plan_waypoints(move_through, graph) << std::endl; waypoints.erase(waypoints.begin(), it+1); event_occurred = true; @@ -1316,21 +1177,17 @@ std::optional ExecutePlan::make( std::vector standbys; auto head = legacy_phases.cbegin(); const auto end = legacy_phases.cend(); - std::stringstream ss; - ss << " ===== Execution plan for " << context->requester_id() << ":"; while (head != end) { if (const auto door = search_for_door_group(head, end, context, plan_id, event_id)) { - ss << "\n -- Move through door"; standbys.push_back(door->group); head = door->tail; } else if (const auto lift = search_for_lift_group( head, end, context, plan_id, event_id, state)) { - ss << "\n -- Move through lift"; standbys.push_back(lift->group); head = lift->tail; } @@ -1338,14 +1195,12 @@ std::optional ExecutePlan::make( { if (head->mutex_group_dependency.has_value()) { - ss << "\n -- Lock mutex group [" << head->mutex_group_dependency->all_groups_str() << "] " << head->mutex_group_dependency->hold_position.transpose(); standbys.push_back(make_wait_for_mutex( context, event_id, head->mutex_group_dependency.value())); } if (head->phase) { - ss << "\n -- " << head->phase->description(); standbys.push_back( [legacy = head->phase, context, event_id](UpdateFn update) { @@ -1356,12 +1211,6 @@ std::optional ExecutePlan::make( if (!head->dependencies.empty()) { - ss << "\n -- wait for:"; - for (const auto& d : head->dependencies) - { - ss << " " << d.on_participant; - } - standbys.push_back(make_wait_for_traffic( context, plan_id, head->dependencies, head->time, event_id)); } @@ -1369,7 +1218,6 @@ std::optional ExecutePlan::make( ++head; } } - std::cout << ss.str() << std::endl; if (tail_period.has_value() && !legacy_phases.empty()) { 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 b4a4fb46c..d674307b4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -396,7 +396,15 @@ void GoToPlace::Active::_find_plan() "Generating plan to move from [" + start_name + "] to [" + goal_name + "]"); const auto& graph = _context->navigation_graph(); - std::cout << _context->requester_id() << " locations " << agv::print_starts(_context->location(), graph) << std::endl; + 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( @@ -438,16 +446,8 @@ void GoToPlace::Active::_find_plan() "Found a plan to move from [" + start_name + "] to [" + goal_name + "]"); - std::stringstream ss; - ss << self->_context->requester_id() << " initial itinerary: "; - for (const auto& r : result->get_itinerary()) - ss << "[" << r.map() << ":" << r.trajectory().size() << "]"; - - ss << " + "; auto full_itinerary = project_itinerary( - *result, self->_followed_by, *self->_context->planner(), &ss); - - std::cout << ss.str() << std::endl; + *result, self->_followed_by, *self->_context->planner()); self->_execute_plan( self->_context->itinerary().assign_plan_id(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index c5b610f30..e2f62db75 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -215,9 +215,6 @@ void LockMutexGroup::Active::_initialize() self->_apply_cumulative_delay(); }); - std::cout << " ===== SETTING MUTEX GROUP FOR " << _context->requester_id().c_str() - << " TO " << _data.all_groups_str() << " AT " << _data.hold_position.transpose() << std::endl; - _listener = _context->request_mutex_groups( _data.mutex_groups, _data.hold_time) .observe_on(rxcpp::identity_same_worker(_context->worker())) @@ -338,8 +335,6 @@ void LockMutexGroup::Active::_initialize() void LockMutexGroup::Active::_schedule( rmf_traffic::schedule::Itinerary itinerary) const { - std::cout << " --- [" << _context->requester_id() << "] resuming with " - << itinerary.size() << " routes" << std::endl; _context->schedule_itinerary(_data.plan_id, std::move(itinerary)); } 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 d6cf36f85..f0acae3e4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -263,33 +263,11 @@ void WaitForTraffic::Active::_consider_going() } bool all_dependencies_reached = true; - std::stringstream ss; - ss << "consider going for [" << _context->requester_id() << "]\n"; for (const auto& dep : _dependencies) { - const auto& d = dep.dependency(); - const auto current = _context->schedule()->get_current_plan_id(d.on_participant); - const auto* p = _context->schedule()->get_current_progress(d.on_participant); - ss << " -- " << d.on_participant << " | " << d.on_plan - << ":" << d.on_route << ":" << d.on_checkpoint - << " vs current " << current.value_or((std::size_t)(-1)); - if (p) - { - for (std::size_t i=0; i < p->size(); ++i) - { - ss << ":[" << i << ":" << (*p)[i] << "]"; - } - } - else - { - ss << "null"; - } - - ss << " | " << dep.reached() << " | " << dep.deprecated() << "\n"; if (!dep.reached() && !dep.deprecated()) all_dependencies_reached = false; } - std::cout << ss.str() << std::endl; if (all_dependencies_reached) { 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 873d305fb..688f06ca1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -129,22 +129,15 @@ void DockRobot::Action::operator()(const Subscriber& s) [s, dock_name = active->_dock_name, context = active->_context, wp = active->_waypoint, plan_id = active->_plan_id]() { - std::cout << "Docking finish callback triggered" << std::endl; context->worker().schedule( [s, dock_name, context, wp, plan_id](const auto&) { - std::cout << "Finished docking [" + context->requester_id() - << "] into dock [" << dock_name << "]" << std::endl; 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()) { - std::cout << "docking reached " << context->participant_id() - << " | " << context->itinerary().current_plan_id() - << ":" << c.route_id << ":" << c.checkpoint_id - << " #" << context->itinerary().progress_version() << std::endl; context->itinerary().reached(plan_id, c.route_id, c.checkpoint_id); } 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 1bf0cf991..aeabf6ea0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -30,14 +30,6 @@ MoveRobot::ActivePhase::ActivePhase( : _context{std::move(context)}, _tail_period(tail_period) { - std::stringstream ss; - ss << "Activating MoveRobot:"; - for (const auto& wp : waypoints) - { - ss << "\n -- " << agv::print_plan_waypoint(wp, _context->navigation_graph(), waypoints.front().time()); - } - std::cout << ss.str() << std::endl; - std::ostringstream oss; const auto dest = destination( waypoints.back(), _context->planner()->get_configuration().graph()); 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 71edf2357..e6553aa2e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -294,8 +294,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (!context->locked_mutex_groups().empty()) { - // std::stringstream ss; - // ss << context->requester_id() << " retaining:"; const auto adjusted_now = now - new_cumulative_delay; const auto& graph = context->navigation_graph(); std::unordered_set retain_mutexes; @@ -310,28 +308,17 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (wp.graph_index().has_value()) { - // if (!graph.get_waypoint(*wp.graph_index()).in_mutex_group().empty()) - // { - // ss << " [wp:" << graph.get_waypoint(*wp.graph_index()).name_or_index() - // << " | " << graph.get_waypoint(*wp.graph_index()).in_mutex_group() << "]"; - // } retain_mutexes.insert( graph.get_waypoint(*wp.graph_index()).in_mutex_group()); } for (const auto& l : wp.approach_lanes()) { - // if (!graph.get_lane(l).properties().in_mutex_group().empty()) - // { - // ss << " [lane:" << l << " | " << << "]"; - // } - retain_mutexes.insert( graph.get_lane(l).properties().in_mutex_group()); } } - // std::cout << ss.str() << std::endl; context->retain_mutex_groups(retain_mutexes); } }); 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 abffa8372..3f5c23e1a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -66,7 +66,7 @@ void RequestLift::ActivePhase::emergency_alarm(bool /*on*/) //============================================================================== void RequestLift::ActivePhase::cancel() { - // GoToPlace / ExecutePlan don't call the cancel function anyway + // GoToPlace and ExecutePlan don't call the cancel function anyway } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp index 642ea4cc2..55dd588b9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.cpp @@ -23,8 +23,7 @@ namespace rmf_fleet_adapter { rmf_traffic::schedule::Itinerary project_itinerary( const rmf_traffic::agv::Plan& starting_from, const std::vector& through_destinations, - const rmf_traffic::agv::Planner& with_planner, - std::stringstream* ss) + const rmf_traffic::agv::Planner& with_planner) { auto itinerary = starting_from.get_itinerary(); auto last_plan = starting_from; @@ -50,11 +49,6 @@ rmf_traffic::schedule::Itinerary project_itinerary( const auto& new_itinerary = last_plan.get_itinerary(); if (new_itinerary.front().map() == itinerary.back().map()) { - if (ss) - { - *ss << "[" << new_itinerary.front().map() << ":" << new_itinerary.front().trajectory().size() - << "]"; - } // We only look at the first route because we're not going to include // any map switches for now. for (const auto& wp : new_itinerary.front().trajectory()) @@ -62,10 +56,6 @@ rmf_traffic::schedule::Itinerary project_itinerary( } else { - if (ss) - { - *ss << "[" << new_itinerary.front().map() << "rejected]"; - } // If the map has switched, let's break out of this loop. break; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp index f6a40d2d2..c8fe77776 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/project_itinerary.hpp @@ -27,8 +27,7 @@ namespace rmf_fleet_adapter { rmf_traffic::schedule::Itinerary project_itinerary( const rmf_traffic::agv::Plan& starting_from, const std::vector& through_destinations, - const rmf_traffic::agv::Planner& with_planner, - std::stringstream* ss = nullptr); + const rmf_traffic::agv::Planner& with_planner); } // namespace rmf_fleet_adapter 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 ddc291738..aa294ca25 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp @@ -396,26 +396,6 @@ class MirrorManager::Implementation try { const rmf_traffic::schedule::Patch patch = convert(msg->patch); - std::stringstream ss; - bool new_progress = false; - for (const auto& c : patch) - { - if (c.progress().has_value()) - { - new_progress = true; - const auto& progress = c.progress()->checkpoints(); - ss << "\n -- #" << c.progress()->version() << " " << c.participant_id() << " |"; - for (std::size_t i=0; i < progress.size(); ++i) - { - ss << " " << i << ":" << progress[i]; - } - } - } - if (new_progress) - { - ss << "\n >>>>>> NEW PROGRESS"; - std::cout << ss.str() << std::endl; - } std::mutex* update_mutex = options.update_mutex(); if (update_mutex) 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 fba0f2c17..1418a86a1 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Node.cpp @@ -287,33 +287,6 @@ void ScheduleNode::setup(const QueryMap& queries) setup_incosistency_pub(); setup_conflict_topics_and_thread(); setup_cull_timer(); - - debug_timer = create_wall_timer( - std::chrono::seconds(2), - [&]() - { - std::stringstream ss; - ss << " ==== PROGRESS"; - for (const auto id : database->participant_ids()) - { - ss << "\n -- #" << database->get_current_progress_version(id) << " " << id << " | plan " - << database->get_current_plan_id(id).value_or((std::size_t)(-1)); - const auto* p = database->get_current_progress(id); - if (p) - { - for (std::size_t i=0; i < p->size(); ++i) - { - ss << " " << i << ":" << (*p)[i]; - } - } - else - { - ss << "null"; - } - } - - std::cout << ss.str() << std::endl; - }); } //============================================================================== diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp index 153082bf0..c5c7a7ac1 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/internal_Node.hpp @@ -544,8 +544,6 @@ class ScheduleNode : public rclcpp::Node // description versions separately from itinerary versions. std::size_t last_known_participants_version = 0; std::size_t current_participants_version = 1; - - rclcpp::TimerBase::SharedPtr debug_timer; }; } // namespace schedule From 3a0772af37b2ff1d8e4d7162abbbbce04c605679 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 5 Dec 2023 09:05:30 +0000 Subject: [PATCH 86/89] Do not replan after a failed negotiation if the agent is stubborn Signed-off-by: Michael X. Grey --- .../agv/FleetUpdateHandle.cpp | 29 +++++++------------ 1 file changed, 11 insertions(+), 18 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 37421607c..729bec85f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1599,21 +1599,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; @@ -1625,11 +1615,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)); From 3254a56059d80122d90303fae8ed78b0cd823213 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 13 Dec 2023 01:48:51 +0800 Subject: [PATCH 87/89] Fix style and tests Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 4 +- .../tasks/ParkRobotIndefinitely.hpp | 8 +- .../src/lift_supervisor/Node.cpp | 12 +- .../src/mutex_group_supervisor/main.cpp | 2 +- .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 65 +++--- .../agv/FleetUpdateHandle.cpp | 194 +++++++++--------- .../rmf_fleet_adapter/agv/RobotContext.cpp | 46 +++-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 48 ++--- .../agv/RobotUpdateHandle.cpp | 23 ++- .../agv/internal_FleetUpdateHandle.hpp | 21 +- .../agv/internal_RobotUpdateHandle.hpp | 3 +- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 40 ++-- .../agv/test/MockAdapter.cpp | 40 ++-- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 123 +++++------ .../rmf_fleet_adapter/events/GoToPlace.cpp | 4 +- .../events/LockMutexGroup.cpp | 106 +++++----- .../events/LockMutexGroup.hpp | 156 +++++++------- .../rmf_fleet_adapter/phases/DockRobot.hpp | 7 +- .../rmf_fleet_adapter/phases/MoveRobot.cpp | 2 +- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 96 ++++----- .../rmf_fleet_adapter/phases/RequestLift.cpp | 39 ++-- .../rmf_fleet_adapter/phases/RequestLift.hpp | 3 +- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 24 +-- .../test/phases/test_RequestLift.cpp | 69 ++----- 24 files changed, 567 insertions(+), 568 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 42782e55b..2be6c0698 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -101,8 +101,8 @@ class EasyFullControl : public std::enable_shared_from_this /// The command execution progress updater. Use this to keep the fleet /// adapter updated on the progress of localizing. using LocalizationRequest = std::function; + Destination location_estimate, + CommandExecution execution)>; /// Add a robot to the fleet once it is available. /// diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp index 277586bc2..01a2d315b 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp @@ -44,12 +44,12 @@ class ParkRobotIndefinitely : public rmf_task::RequestFactory /// 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); + 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; + const rmf_task::State& state) const final; class Implementation; private: diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index 24e81369f..77c05fdcb 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -105,12 +105,12 @@ 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->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() - ); + _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 { diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp index b09a80984..4f07df44d 100644 --- a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -195,7 +195,7 @@ class Node : public rclcpp::Node } std::unordered_set normalized_groups; - for (auto i=claims.begin(); i != claims.end(); ++i) + for (auto i = claims.begin(); i != claims.end(); ++i) { auto j = i; ++j; 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 1713900b1..bc9db015c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -313,7 +313,8 @@ class EasyFullControl::CommandExecution::Implementation::Data } const auto& graph = planner->get_configuration().graph(); - const rmf_traffic::agv::LaneClosure* closures = context->get_lane_closures(); + 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]; @@ -461,7 +462,7 @@ class EasyFullControl::CommandExecution::Implementation::Data { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, graph); + << " starts:" << print_starts(starts, graph); std::cout << ss.str() << std::endl; } context->set_location(starts); @@ -472,8 +473,8 @@ class EasyFullControl::CommandExecution::Implementation::Data { 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"; + << 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) { @@ -627,12 +628,12 @@ auto EasyFullControl::CommandExecution::Implementation::make_hold( std::function finisher) -> CommandExecution { auto update_fn = [ - w_context = context->weak_from_this(), - expected_time, - plan_id + w_context = context->weak_from_this(), + expected_time, + plan_id ]( - const std::string& map, - Eigen::Vector3d location) + const std::string& map, + Eigen::Vector3d location) { const auto context = w_context.lock(); if (!context) @@ -644,7 +645,8 @@ auto EasyFullControl::CommandExecution::Implementation::make_hold( { if (context->debug_positions) { - std::cout << "Searching for location from " << __FILE__ << "|" << __LINE__ << std::endl; + std::cout << "Searching for location from " << __FILE__ << "|" << + __LINE__ << std::endl; } nav_params->search_for_location(map, location, *context); } @@ -1023,7 +1025,8 @@ void EasyCommandHandle::follow_new_path( { for (const auto& l : current_location) { - if (nav_params->in_same_stack(*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()) { @@ -1233,7 +1236,8 @@ void EasyCommandHandle::follow_new_path( planned_wait_time, std::nullopt, nav_params, - [next_arrival_estimator_, target_index, target_p](rmf_traffic::Duration dt) + [next_arrival_estimator_, target_index, + target_p](rmf_traffic::Duration dt) { next_arrival_estimator_(target_index, dt); } @@ -1552,7 +1556,8 @@ 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; } } @@ -2011,7 +2016,7 @@ EasyFullControl::FleetConfiguration::from_config_files( { finishing_request = std::make_shared( - "idle", nullptr); + "idle", nullptr); std::cout << "Fleet is configured to perform ParkRobot as finishing request" << std::endl; @@ -2220,11 +2225,11 @@ EasyFullControl::FleetConfiguration::from_config_files( } auto config = RobotConfiguration( - std::move(chargers), - responsive_wait, - max_merge_waypoint_distance, - max_merge_lane_distance, - min_lane_length); + 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); } else @@ -2690,20 +2695,20 @@ auto EasyFullControl::add_robot( 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()) + ](Destination estimate, CommandExecution execution) { - auto transformed_estimate = estimate; - Destination::Implementation::get(transformed_estimate) + 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); - } - }; + inner(transformed_estimate, execution); + } + }; } const auto& fleet_impl = 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 729bec85f..2e148d22d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -351,7 +351,8 @@ class AllocateTasks : public std::enable_shared_from_this { std::string error_str = "[TaskPlanner] Failed to compute assignments for task_id [" + id - + "] due to insufficient initial battery charge for all robots in this " + + + "] due to insufficient initial battery charge for all robots in this " "fleet."; RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); @@ -375,7 +376,8 @@ class AllocateTasks : public std::enable_shared_from_this else { std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id + "]"; + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "]"; RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); errors.push_back( @@ -470,7 +472,7 @@ 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); + rmf_api_msgs::schemas::task_request); try { @@ -510,108 +512,111 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( auto receive_allocation = [w = weak_self, respond, task_id]( AllocateTasks::Result result) - { - const auto self = w.lock(); - if (!self) - return; + { + const auto self = w.lock(); + if (!self) + return; - auto allocation_result = result.assignments; - if (!allocation_result.has_value()) - return respond({std::nullopt, std::move(result.errors)}); + auto allocation_result = result.assignments; + if (!allocation_result.has_value()) + return respond({std::nullopt, std::move(result.errors)}); - const auto& assignments = allocation_result.value(); + const auto& assignments = allocation_result.value(); - const double cost = self->_pimpl->task_planner->compute_cost(assignments); + const double cost = self->_pimpl->task_planner->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) - { - debug_stream << "--Agent: " << i << std::endl; - for (const auto& a : assignments[i]) + // 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) { - 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 << "--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; + debug_stream << " ----------------------" << std::endl; - RCLCPP_DEBUG( - self->_pimpl->node->get_logger(), - "%s", - debug_stream.str().c_str()); + 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) - { - robot_name_map.insert({index, t.first->name()}); - ++index; - } + // 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& assignment : agent) + std::optional robot_name; + std::optional finish_time; + index = 0; + for (const auto& agent : assignments) { - if (assignment.request()->booking()->id() == task_id) + for (const auto& assignment : agent) { - finish_time = assignment.finish_state().time().value(); - if (robot_name_map.find(index) != robot_name_map.end()) - robot_name = robot_name_map[index]; - break; + 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; } - ++index; - } - 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.")); + 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.")); - return respond({std::nullopt, std::move(result.errors)}); - } + return respond({std::nullopt, std::move(result.errors)}); + } - // 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) - }); + // 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); + 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 + self->_pimpl->bid_notice_assignments.insert({task_id, assignments}); + }; calculate_bid_subscription = rmf_rxcpp::make_job( calculate_bid) @@ -749,8 +754,8 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( // 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); + nullptr, aggregate_expectations(), *task_planner, node) + .run(dispatch_ack.errors); if (!replan_results) { @@ -1154,7 +1159,8 @@ void FleetUpdateHandle::Implementation::update_fleet_logs() const } //============================================================================== -void FleetUpdateHandle::Implementation::handle_emergency(const bool is_emergency) +void FleetUpdateHandle::Implementation::handle_emergency( + const bool is_emergency) { if (is_emergency == emergency_active) return; @@ -1641,14 +1647,14 @@ void FleetUpdateHandle::add_robot( broadcast_client = fleet->_pimpl->broadcast_client; const auto mgr = TaskManager::make( - context, - broadcast_client, - std::weak_ptr(fleet)); + 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()); + ->unregistered_charging_assignments.find(context->name()); if (c_it != fleet->_pimpl->unregistered_charging_assignments.end()) { const auto& charging = c_it->second; 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 861374dc8..12802a914 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -53,7 +53,7 @@ void NavParams::search_for_location( { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, graph); + << " starts:" << print_starts(starts, graph); std::cout << ss.str() << std::endl; } context.set_location(std::move(starts)); @@ -63,8 +63,8 @@ void NavParams::search_for_location( 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; + << map << " <" << position.block<2, 1>(0, 0).transpose() + << "> orientation " << position[2] * 180.0 / M_PI << std::endl; } context.set_lost(Location { now, map, position }); } @@ -174,7 +174,7 @@ rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( rmf_traffic::agv::Plan::StartSet locations) const { std::vector remove; - for (std::size_t i=0; i < locations.size(); ++i) + for (std::size_t i = 0; i < locations.size(); ++i) { rmf_traffic::agv::Plan::Start& location = locations[i]; std::optional waypoint_opt; @@ -296,18 +296,18 @@ rmf_traffic::agv::Plan::StartSet NavParams::_lift_boundary_filter( const Eigen::Vector2d p = location.location().value(); const auto robot_inside_lift = [&]() - -> rmf_traffic::agv::Graph::LiftPropertiesPtr + -> rmf_traffic::agv::Graph::LiftPropertiesPtr + { + for (const auto& lift : graph.all_known_lifts()) { - 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; - } + // We assume lifts never overlap so we will return the first + // positive hit. + if (lift->is_in_lift(p)) + return lift; + } - return nullptr; - }(); + return nullptr; + }(); const auto& wp = graph.get_waypoint(location.waypoint()); const auto lift = wp.in_lift(); @@ -375,7 +375,7 @@ Eigen::Vector3d RobotContext::position() const } throw std::runtime_error( - "No location information is available for [" + requester_id() + "]"); + "No location information is available for [" + requester_id() + "]"); } //============================================================================== @@ -392,7 +392,7 @@ const std::string& RobotContext::map() const } throw std::runtime_error( - "No location information is available for [" + requester_id() + "]"); + "No location information is available for [" + requester_id() + "]"); } //============================================================================== @@ -428,7 +428,8 @@ void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) { if (debug_positions) { - std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST" << std::endl; + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST" << + std::endl; } set_lost(std::nullopt); return; @@ -1031,6 +1032,7 @@ std::shared_ptr RobotContext::set_lift_destination( }); _initial_time_idle_outside_lift = std::nullopt; + _publish_lift_destination(); return _lift_destination; } @@ -1172,7 +1174,7 @@ void RobotContext::schedule_itinerary( ss_sizes.str().c_str(), requester_id().c_str(), *plan_id); - break; + break; } scheduled = itinerary().set(*plan_id, new_itinerary); @@ -1361,6 +1363,12 @@ void RobotContext::_check_lift_state( return; } + _publish_lift_destination(); +} + +//============================================================================== +void RobotContext::_publish_lift_destination() +{ rmf_lift_msgs::msg::LiftRequest msg; msg.lift_name = _lift_destination->lift_name; msg.destination_floor = _lift_destination->destination_floor; @@ -1489,7 +1497,7 @@ void RobotContext::_publish_mutex_group_requests() // 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()) + || !_locked_mutex_groups.empty()) { auto warning = [&](const std::string& name) { 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 eaf7a0906..9c82be580 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -122,7 +122,7 @@ inline std::string print_waypoint( 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() << "]"; + << wp.name_or_index() << "]"; return ss.str(); } @@ -137,7 +137,7 @@ inline std::string print_plan_waypoint( if (wp.graph_index().has_value()) ss << " #" << *wp.graph_index(); ss << " <" << wp.position().transpose() - << "> yaw=" << wp.position()[2] * 180.0 / M_PI; + << "> yaw=" << wp.position()[2] * 180.0 / M_PI; if (wp.event()) { EventPrinter event; @@ -195,7 +195,7 @@ inline std::string print_lane( 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); + << " -> " << print_lane_node(lane.exit(), graph); return ss.str(); } @@ -216,7 +216,7 @@ inline std::string print_starts( if (l.waypoint() != lane.exit().waypoint_index()) { ss << " !! MISMATCH BETWEEN KEY WAYPOINT AND LANE EXIT: key " - << l.waypoint() << " vs exit " << lane.exit().waypoint_index(); + << l.waypoint() << " vs exit " << lane.exit().waypoint_index(); } } else @@ -720,35 +720,35 @@ class RobotContext 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; + { + const auto self = w.lock(); + if (!self) + return; - self->_check_lift_state(*msg); - }); + 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; + { + const auto self = w.lock(); + if (!self) + return; - self->_check_door_supervisor(*msg); - }); + 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; + { + const auto self = w.lock(); + if (!self) + return; - self->_check_mutex_groups(*msg); - }); + self->_check_mutex_groups(*msg); + }); context->_mutex_group_heartbeat = context->_node->try_create_wall_timer( std::chrono::seconds(2), @@ -845,9 +845,11 @@ class RobotContext 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::optional + _initial_time_idle_outside_lift; std::shared_ptr _lift_stubbornness; bool _lift_arrived = false; 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 6a3f20681..46fbb44f1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -86,7 +86,7 @@ void RobotUpdateHandle::update_position( { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, context->navigation_graph()); + << " starts:" << print_starts(starts, context->navigation_graph()); std::cout << ss.str() << std::endl; } context->set_location(starts); @@ -129,7 +129,7 @@ void RobotUpdateHandle::update_position( { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, context->navigation_graph()); + << " starts:" << print_starts(starts, context->navigation_graph()); std::cout << ss.str() << std::endl; } context->set_location(std::move(starts)); @@ -156,7 +156,7 @@ void RobotUpdateHandle::update_position( { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, context->navigation_graph()); + << " starts:" << print_starts(starts, context->navigation_graph()); std::cout << ss.str() << std::endl; } context->set_location(std::move(starts)); @@ -190,13 +190,15 @@ 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; + << 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 }); }); @@ -210,7 +212,7 @@ void RobotUpdateHandle::update_position( { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, context->navigation_graph()); + << " starts:" << print_starts(starts, context->navigation_graph()); std::cout << ss.str() << std::endl; } context->set_location(std::move(starts)); @@ -231,7 +233,7 @@ void RobotUpdateHandle::update_position( { std::stringstream ss; ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, context->navigation_graph()); + << " starts:" << print_starts(starts, context->navigation_graph()); std::cout << ss.str() << std::endl; } context->set_location(starts); @@ -246,7 +248,7 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( if (const auto context = _pimpl->get_context()) { context->worker().schedule([charger_wp, w = context->weak_from_this()]( - const auto&) + const auto&) { const auto self = w.lock(); if (!self) @@ -1183,7 +1185,8 @@ void ScheduleOverride::overridden_update( if (context->debug_positions) { - std::cout << "Search for location from " << __FILE__ << "|" << __LINE__ << std::endl; + 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/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 49a13f533..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 @@ -307,7 +307,7 @@ class FleetUpdateHandle::Implementation // Keep track of charging assignments for robots that have not been registered // yet. std::unordered_map - unregistered_charging_assignments; + unregistered_charging_assignments; using DockParamMap = std::unordered_map< @@ -366,7 +366,8 @@ class FleetUpdateHandle::Implementation handle->_pimpl->add_standard_tasks(); - handle->_pimpl->emergency_obs = handle->_pimpl->emergency_publisher.get_observable(); + 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( @@ -581,14 +582,14 @@ 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); - }); + 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); 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 02baca36f..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 @@ -195,7 +195,8 @@ class RobotUpdateHandle::ActionExecution::Implementation { if (context->debug_positions) { - std::cout << "Searching for location from " << __FILE__ << "|" << __LINE__ << std::endl; + 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 fe14ba0d5..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 @@ -43,10 +43,12 @@ rmf_traffic::agv::Graph parse_graph( 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; + << + "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 { @@ -68,7 +70,7 @@ rmf_traffic::agv::Graph parse_graph( dims_yaml[1].as()); graph.set_known_lift(rmf_traffic::agv::Graph::LiftProperties( - name, location, orientation, dimensions)); + name, location, orientation, dimensions)); } } @@ -76,10 +78,12 @@ rmf_traffic::agv::Graph parse_graph( 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; + << + "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 { @@ -215,9 +219,9 @@ rmf_traffic::agv::Graph parse_graph( 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."); + "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); } @@ -374,7 +378,8 @@ rmf_traffic::agv::Graph parse_graph( // 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()); + 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); @@ -411,7 +416,7 @@ rmf_traffic::agv::Graph parse_graph( if (const YAML::Node mutex_yaml = options["mutex"]) { graph_lane.properties() - .set_in_mutex_group(mutex_yaml.as()); + .set_in_mutex_group(mutex_yaml.as()); } } vnum += vnum_temp; @@ -452,8 +457,8 @@ rmf_traffic::agv::Graph parse_graph( 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)); + "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(); @@ -473,7 +478,8 @@ rmf_traffic::agv::Graph parse_graph( 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; + 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..e5c316c7c 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,28 @@ 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/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index b17feb0f5..e8127f8ef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -46,7 +46,8 @@ std::string print_plan_waypoints( { std::stringstream ss; for (const auto& wp : waypoints) - ss << "\n -- " << agv::print_plan_waypoint(wp, graph, waypoints.front().time()); + ss << "\n -- " << agv::print_plan_waypoint(wp, graph, + waypoints.front().time()); return ss.str(); } @@ -123,7 +124,7 @@ void truncate_arrival( t.erase(t.begin() + (int)c.checkpoint_id, t.end()); } - for (std::size_t i=0; i < previous_itinerary.size(); ++i) + for (std::size_t i = 0; i < previous_itinerary.size(); ++i) { const auto& t = previous_itinerary.at(i).trajectory(); if (t.size() < 2) @@ -348,10 +349,10 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor rmf_traffic::Time _event_start_time; PlanIdPtr _plan_id; std::function&, - const rmf_traffic::agv::Plan::Waypoint&)> _make_current_mutex_groups; + const std::unordered_set&, + const rmf_traffic::agv::Plan::Waypoint&)> _make_current_mutex_groups; std::function>( - const rmf_traffic::agv::Plan::Waypoint&)> _get_new_mutex_group; + const rmf_traffic::agv::Plan::Waypoint&)> _get_new_mutex_group; std::shared_ptr& _previous_itinerary; const rmf_traffic::schedule::Itinerary& _full_itinerary; bool& _continuous; @@ -410,7 +411,7 @@ std::optional search_for_door_group( if (it->mutex_group_dependency.has_value()) { door_group.push_back(make_wait_for_mutex( - context, id, it->mutex_group_dependency.value())); + context, id, it->mutex_group_dependency.value())); } if (it->phase) @@ -528,7 +529,7 @@ std::optional search_for_lift_group( if (it->mutex_group_dependency.has_value()) { lift_group.push_back(make_wait_for_mutex( - context, event_id, it->mutex_group_dependency.value())); + context, event_id, it->mutex_group_dependency.value())); } if (it->phase) @@ -723,7 +724,7 @@ std::optional ExecutePlan::make( { const Eigen::Vector2d p0 = plan.get_waypoints().front().position().block<2, 1>(0, 0); - const auto t0 = plan.get_waypoints().front().time(); + const auto t0 = plan.get_waypoints().front().time(); const auto first_graph_wp = [&]() -> std::optional { @@ -847,66 +848,66 @@ std::optional ExecutePlan::make( 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) + 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()) { - 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) { - hold_map = - graph.get_waypoint(*wp.graph_index()).get_map_name(); - break; + 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()); + 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 - }; + 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; + previous_itinerary = data.resume_itinerary; - return data; - }; + return data; + }; const auto get_new_mutex_groups = [&]( - const rmf_traffic::agv::Plan::Waypoint& wp) + const rmf_traffic::agv::Plan::Waypoint& wp) { std::unordered_set new_mutex_groups; if (wp.graph_index().has_value()) @@ -964,7 +965,8 @@ std::optional ExecutePlan::make( bool event_occurred = false; for (; it != waypoints.end(); ++it) { - const auto [mutex_group_change, new_mutex_groups] = get_new_mutex_groups(*it); + const auto [mutex_group_change, new_mutex_groups] = get_new_mutex_groups( + *it); if (mutex_group_change) { if (move_through.size() > 1) @@ -1097,7 +1099,8 @@ std::optional ExecutePlan::make( { // 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) + "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."); } @@ -1196,7 +1199,7 @@ std::optional ExecutePlan::make( if (head->mutex_group_dependency.has_value()) { standbys.push_back(make_wait_for_mutex( - context, event_id, head->mutex_group_dependency.value())); + context, event_id, head->mutex_group_dependency.value())); } if (head->phase) 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 d674307b4..671e62b99 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -398,8 +398,8 @@ void GoToPlace::Active::_find_plan() 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); + << "] to [" << goal_name << "] from one of these locations:" + << agv::print_starts(_context->location(), graph); RCLCPP_INFO( _context->node()->get_logger(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp index e2f62db75..8c1dba8f1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -237,20 +237,20 @@ void LockMutexGroup::Active::_initialize() if (delay > std::chrono::seconds(2)) { const auto start = [&]() - -> std::optional + -> std::optional + { + for (const auto& wp : self->_data.waypoints) { - for (const auto& wp : self->_data.waypoints) + if (wp.graph_index().has_value()) { - if (wp.graph_index().has_value()) - { - return rmf_traffic::agv::Plan::Start( - self->_context->now(), - *wp.graph_index(), - wp.position()[2]); - } + return rmf_traffic::agv::Plan::Start( + self->_context->now(), + *wp.graph_index(), + wp.position()[2]); } - return std::nullopt; - }(); + } + return std::nullopt; + }(); if (start.has_value()) { @@ -264,53 +264,53 @@ void LockMutexGroup::Active::_initialize() 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) + 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)) { - 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. + // 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); - self->_context->request_replan(); - }); + 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() - ]() + 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()) { - if (const auto service = weak_service.lock()) - { - service->interrupt(); - } - }); + service->interrupt(); + } + }); return; } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp index 95a1ce944..1d8c75b35 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -33,84 +33,84 @@ 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; - }; + 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 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 688f06ca1..8bd1113c5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -120,8 +120,8 @@ void DockRobot::Action::operator()(const Subscriber& s) LegacyTask::StatusMsg status; status.state = LegacyTask::StatusMsg::STATE_ACTIVE; status.status = "Docking [" + active->_context->requester_id() + - "] into dock [" - + active->_dock_name + "]"; + "] into dock [" + + active->_dock_name + "]"; s.on_next(status); active->_context->command()->dock( @@ -138,7 +138,8 @@ void DockRobot::Action::operator()(const Subscriber& s) status.state = LegacyTask::StatusMsg::STATE_COMPLETED; for (const auto& c : wp.arrival_checkpoints()) { - context->itinerary().reached(plan_id, c.route_id, c.checkpoint_id); + context->itinerary().reached(plan_id, c.route_id, + c.checkpoint_id); } if (wp.graph_index().has_value()) 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 aeabf6ea0..92208d4fe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -94,7 +94,7 @@ MoveRobot::PendingPhase::PendingPhase( const auto dest = destination( _waypoints.back(), _context->planner()->get_configuration().graph()); oss << "Move to " << dest << " <" << _waypoints.back().position().transpose() - << "> through " << _waypoints.size() << " points"; + << "> through " << _waypoints.size() << " points"; _description = oss.str(); } 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 e6553aa2e..575730fcd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -140,41 +140,42 @@ void MoveRobot::Action::operator()(const Subscriber& s) return; _context->worker().schedule([w = weak_from_this(), s](const auto&) - { - const auto self = w.lock(); - if (!self) - return; - - 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; - - const auto now = self->_context->node()->now(); - if (now < self->_last_update_rostime + self->_update_timeout) + { + const auto self = w.lock(); + if (!self) + return; + + 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()]() { - // The simulation is paused or running slowly, so we should allow more - // patience before assuming that there's been a timeout. - return; - } + const auto self = w.lock(); + if (!self) + return; - self->_last_update_rostime = now; + 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(); - }); + // 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(); + }); - const auto update = [ + const auto update = [ s, w_action = self->weak_from_this(), r = self->_context->requester_id() @@ -205,7 +206,8 @@ void MoveRobot::Action::operator()(const Subscriber& s) ]( const auto&) { - if (const auto c = context->itinerary().cumulative_delay(plan_id)) + if (const auto c = + context->itinerary().cumulative_delay(plan_id)) { context->itinerary().cumulative_delay(plan_id, *c + bump); } @@ -299,7 +301,9 @@ void MoveRobot::Action::operator()(const Subscriber& s) std::unordered_set retain_mutexes; for (const auto& wp : self->_waypoints) { - const auto s_100 = (int)(rmf_traffic::time::to_seconds(adjusted_now - wp.time()) * 100); + 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) { @@ -324,7 +328,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) }); }; - const auto finish = [ + const auto finish = [ s, w = self->weak_from_this(), name = self->_context->requester_id() @@ -362,24 +366,24 @@ void MoveRobot::Action::operator()(const Subscriber& s) } }; - 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&) + 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&) + }, + [worker = self->_context->worker(), finish]() + { + worker.schedule([finish](const auto&) { finish(); }); - }); - }); + }); + }); } } // namespace phases 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 3f5c23e1a..57272a2db 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -116,7 +116,7 @@ void RequestLift::ActivePhase::_init_obs() self->_context->schedule_itinerary( self->_data.plan_id, *self->_data.resume_itinerary); const auto delay = - self->_context->now() - self->_data.expected_finish; + self->_context->now() - self->_data.expected_finish; self->_context->itinerary().cumulative_delay( *self->_data.plan_id, delay); } @@ -210,32 +210,33 @@ void RequestLift::ActivePhase::_init_obs() if (me->_data.localize_after.has_value()) { auto finish = [s, worker = me->_context->worker(), weak]() + { + worker.schedule([s, weak](const auto&) { - worker.schedule([s, weak](const auto&) + if (const auto me = weak.lock()) + { + if (!me->_finish()) { - if (const auto me = weak.lock()) - { - if (!me->_finish()) - { - return; - } - } + return; + } + } - s.on_completed(); - }); - }; + s.on_completed(); + }); + }; auto cmd = agv::EasyFullControl - ::CommandExecution::Implementation::make_hold( - me->_context, - me->_data.expected_finish, - *me->_data.plan_id, - std::move(finish)); + ::CommandExecution::Implementation::make_hold( + me->_context, + me->_data.expected_finish, + *me->_data.plan_id, + std::move(finish)); agv::Destination::Implementation::get(*me->_data.localize_after) - .position = me->_context->position(); + .position = me->_context->position(); - if (me->_context->localize(*me->_data.localize_after, std::move(cmd))) + 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), 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 45222d240..cc6b9233d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -40,7 +40,8 @@ struct RequestLift Located located; PlanIdPtr plan_id; std::optional localize_after = std::nullopt; - std::shared_ptr resume_itinerary = nullptr; + std::shared_ptr resume_itinerary = + nullptr; std::optional hold_point = std::nullopt; }; 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 da5ea7d8b..863704d0b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -44,7 +44,7 @@ rmf_traffic::Duration estimate_charge_time( { const double delta_soc = recharged_soc - initial_soc; const double dt = (3600 * delta_soc * battery_system.capacity()) / - battery_system.charging_current(); + battery_system.charging_current(); return rmf_traffic::time::from_seconds(dt); } @@ -187,8 +187,8 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event const auto header_go_to = rmf_task_sequence::events::GoToPlace::Description::make( - context->dedicated_charging_wp())->generate_header( - initial_state, parameters); + context->dedicated_charging_wp())->generate_header( + initial_state, parameters); duration_estimate += header_go_to.original_duration_estimate(); } } @@ -460,7 +460,7 @@ class ChargeBatteryEvent : public rmf_task_sequence::Event if (!indefinite) { recharged_soc = context->task_planner() - ->configuration().constraints().recharge_soc(); + ->configuration().constraints().recharge_soc(); } auto legacy = phases::WaitForCharge::make( @@ -732,20 +732,20 @@ rmf_task::ConstRequestPtr ParkRobotIndefinitely::make_request( auto desc = rmf_task_sequence::Task::Builder() .add_phase(rmf_task_sequence::phases::SimplePhase::Description::make( - phase_desc), {}) + 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_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); + std::move(id), + now, + nullptr, + _pimpl->requester, + now, + true); return std::make_shared( std::move(booking), diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index 5ce6a23bf..4290f8629 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; @@ -77,36 +79,6 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") }); 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( @@ -131,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]() { @@ -142,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( @@ -200,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(); From da88b44dedc2f128da811bfda55921c08d953d59 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 13 Dec 2023 02:17:39 +0800 Subject: [PATCH 88/89] Fix style inside of tests Signed-off-by: Michael X. Grey --- .../agv/test/MockAdapter.cpp | 27 ++++++++++--------- .../test/phases/test_RequestLift.cpp | 12 ++++----- 2 files changed, 21 insertions(+), 18 deletions(-) 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 e5c316c7c..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 @@ -352,25 +352,28 @@ void MockAdapter::dispatch_task( fimpl.bid_notice_cb( bid, - [fimpl = &fimpl, task_id](const rmf_task_ros2::bidding::Response& response) + [fimpl = &fimpl, + task_id](const rmf_task_ros2::bidding::Response& response) { 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; - }); + { + 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::cout << "Fleet [" << fimpl->name << + "] rejected the task request" << std::endl; } }); diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index 4290f8629..8e2d23798 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -106,13 +106,13 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") 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)); - } - }); + 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]() { From e4d53e08b7f6aeeec44fa6c2a22d93f07e371a22 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 13 Dec 2023 15:27:12 +0800 Subject: [PATCH 89/89] Periodically log when a robot is being blocked at a mutex or a lift Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 52 +++++++++++++++++-- 1 file changed, 49 insertions(+), 3 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 12802a914..0abfd7165 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1357,10 +1357,15 @@ void RobotContext::_check_lift_state( _lift_destination->destination_floor == state.current_floor; } } - - if (!_lift_destination) + else if (_lift_destination && _lift_destination->lift_name == state.lift_name) { - return; + 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(); @@ -1369,6 +1374,11 @@ void RobotContext::_check_lift_state( //============================================================================== 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; @@ -1424,7 +1434,43 @@ void RobotContext::_check_mutex_groups( 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) {