From adfd0413a4b5a986c872fcafca564d514fc49ac9 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 11 Nov 2024 16:24:33 +0800 Subject: [PATCH 1/3] Debugged lift usage disruption issue Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 31 +++++++++++++++-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 12 +++++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 27 +++++++++++++++ .../rmf_fleet_adapter/events/GoToPlace.cpp | 4 +++ .../events/LegacyPhaseShim.cpp | 5 +++ .../events/WaitForTraffic.cpp | 18 +++++++++- .../rmf_fleet_adapter/phases/RequestLift.cpp | 33 ++++++++++++++++--- 7 files changed, 121 insertions(+), 9 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index caf007a13..66acadcd5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1145,6 +1145,20 @@ const LiftDestination* RobotContext::current_lift_destination() const return _lift_destination.get(); } +//============================================================================== +bool RobotContext::has_lift_arrived( + const std::string& lift_name, + const std::string& destination_floor) const +{ + if (!_lift_destination) + return false; + + if (!_lift_destination->matches(lift_name, destination_floor)) + return false; + + return _lift_arrived; +} + //============================================================================== std::shared_ptr RobotContext::set_lift_destination( std::string lift_name, @@ -1465,6 +1479,20 @@ void RobotContext::_release_door(const std::string& door_name) _holding_door = std::nullopt; } +//============================================================================== +void RobotContext::_set_lift_arrived( + const std::string& lift_name, + const std::string& destination_floor) +{ + if (!_lift_destination) + return; + + if (!_lift_destination->matches(lift_name, destination_floor)) + return; + + _lift_arrived = true; +} + //============================================================================== void RobotContext::_check_lift_state( const rmf_lift_msgs::msg::LiftState& state) @@ -1558,9 +1586,6 @@ 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; } } else if (_lift_destination && _lift_destination->lift_name == state.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 155b86f16..573525c23 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -688,6 +688,12 @@ class RobotContext /// Get the current lift destination request for this robot const LiftDestination* current_lift_destination() const; + /// Check whether the lift has arrived at its current destination with the + /// correct session ID + bool has_lift_arrived( + const std::string& lift_name, + const std::string& destination_floor) 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, @@ -783,6 +789,12 @@ class RobotContext /// Release a door. This should only be used by DoorClose void _release_door(const std::string& door_name); + /// This should only be called by RequestLift to notify the context that the + /// lift successfully arrived for its current destination request. + void _set_lift_arrived( + const std::string& lift_name, + const std::string& destination_name); + template static std::shared_ptr make(Args&&... args) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 2e7c99a52..0a03d945f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -665,6 +665,23 @@ class DoorFinder : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftSessionEnd& e) final {} }; +//============================================================================== +void print_events( + std::stringstream& seq, + rmf_task::Event::ConstStatePtr state, + std::size_t depth +) { + rmf_task::VersionedString::Reader reader; + seq << "\n -- "; + for (std::size_t i=0; i < depth; ++i) { + seq << " "; + } + seq << "[" << state << "] " << *reader.read(state->name()) << ": " << *reader.read(state->detail()); + for (const auto& d : state->dependencies()) { + print_events(seq, d, depth+1); + } +} + //============================================================================== std::optional ExecutePlan::make( agv::RobotContextPtr context, @@ -681,6 +698,11 @@ std::optional ExecutePlan::make( if (plan.get_waypoints().empty()) return std::nullopt; + std::stringstream ss; + ss << "Executing plan for " << context->requester_id() + << print_plan_waypoints(plan.get_waypoints(), context->navigation_graph()); + std::cout << ss.str() << std::endl; + auto plan_id = std::make_shared(recommended_plan_id); const auto& graph = context->navigation_graph(); LegacyPhases legacy_phases; @@ -1311,6 +1333,11 @@ std::optional ExecutePlan::make( rmf_task_sequence::events::Bundle::Type::Sequence, standbys, state, std::move(update))->begin([]() {}, std::move(finished)); + std::stringstream seq; + seq << "Event sequence for " << context->requester_id(); + print_events(seq, sequence->state(), 0); + std::cout << seq.str() << std::endl; + return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 846945bc1..39e42d679 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -671,6 +671,10 @@ void GoToPlace::Active::_execute_plan( .name_or_index().c_str(), _context->requester_id().c_str()); + if (_execution.has_value()) + { + std::cout << " >>>>>> Replacing execution for " << _context->requester_id() << std::endl; + } _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp index d95c99905..288952e91 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp @@ -153,6 +153,11 @@ auto LegacyPhaseShim::Active::make( { if (self->_finished) { + rmf_task::VersionedString::Reader reader; + std::cout << " !!!!!!!!!!!!!!!!!!!! TRIGGERING LEGACY FINISHED FOR [" + << self->_state << "] " + << *reader.read(self->_state->name()) << ": " << *reader.read(self->_state->detail()) + << std::endl; // We don't change the event status here because that should have been // done in the task summary update, and from here we don't know what // kind of finish the legacy phase may have had (e.g. completed, 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 f0acae3e4..9376a4d7b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -29,13 +29,29 @@ auto WaitForTraffic::Standby::make( const AssignIDPtr& id, std::function update) -> std::shared_ptr { + std::stringstream ss; + ss << "["; + for (const auto& dep : dependencies) + { + const auto participant = context->schedule()->get_participant(dep.on_participant); + if (participant) + { + ss << " " << participant->name(); + } + else + { + ss << " "; + } + } + ss << " ]"; + auto standby = std::make_shared(); standby->_context = std::move(context); standby->_plan_id = plan_id; standby->_dependencies = std::move(dependencies); standby->_expected_time = expected_time; standby->_state = rmf_task::events::SimpleEventState::make( - id->assign(), "Wait for traffic", "", + id->assign(), "Wait for traffic", ss.str(), rmf_task::Event::Status::Standby, {}, standby->_context->clock()); standby->_update = std::move(update); 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 3ac92cc12..365ce25f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -87,7 +87,7 @@ RequestLift::ActivePhase::ActivePhase( _data(std::move(data)) { std::ostringstream oss; - oss << "Requesting lift [" << lift_name << "] to [" << destination << "]"; + oss << "Requesting lift [" << _lift_name << "] to [" << _destination << "]"; _description = oss.str(); } @@ -97,6 +97,9 @@ void RequestLift::ActivePhase::_init_obs() { using rmf_lift_msgs::msg::LiftState; + std::cout << " >>> INITIALIZING RequestLift for " << _context->requester_id() + << ": " << _description << std::endl; + if (_data.located == Located::Inside) { // If the robot is requesting from the inside, then we should update the @@ -110,12 +113,12 @@ void RequestLift::ActivePhase::_init_obs() _context->set_final_lift_destination(*_data.final_lift_destination); } - if (_data.located == Located::Outside && _context->current_lift_destination()) + if (_data.located == Located::Outside) { // 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)) + if (_context->has_lift_arrived(_lift_name, _destination)) { _obs = rxcpp::observable<>::create( [w = weak_from_this()](rxcpp::subscriber s) @@ -222,7 +225,10 @@ void RequestLift::ActivePhase::_init_obs() } return true; })) - .take_until(_cancelled.get_observable().filter([](auto b) { return b; })) + .take_until(_cancelled.get_observable().filter([c = _context](auto b) { + std::cout << " ================= CANCELLED TRIGGERED FOR " << c->requester_id() << ": " << b << std::endl; + return b; + })) .concat(rxcpp::observable<>::create( [weak = weak_from_this()](const auto& s) { @@ -238,6 +244,10 @@ void RequestLift::ActivePhase::_init_obs() { if (const auto me = weak.lock()) { + std::cout << "Calling RequestLift finish for " + << me->_context->requester_id() + << " | " << me->_description + << ": " << __LINE__ << std::endl; if (!me->_finish()) { return; @@ -281,6 +291,10 @@ void RequestLift::ActivePhase::_init_obs() "process is finished.", me->_context->requester_id().c_str()); + std::cout << "Calling RequestLift finish for " + << me->_context->requester_id() + << " | " << me->_description + << ": " << __LINE__ << std::endl; if (me->_finish()) s.on_completed(); }); @@ -288,6 +302,10 @@ void RequestLift::ActivePhase::_init_obs() } } + std::cout << "Calling RequestLift finish for " + << me->_context->requester_id() + << " | " << me->_description + << ": " << __LINE__ << std::endl; if (me->_finish()) s.on_completed(); })); @@ -427,6 +445,7 @@ void RequestLift::ActivePhase::_do_publish() //============================================================================== bool RequestLift::ActivePhase::_finish() { + std::cout << "RequestLift finished has been triggered for " << _context->requester_id() << std::endl; // 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 @@ -444,6 +463,10 @@ bool RequestLift::ActivePhase::_finish() // releasing the lift prematurely. _context->set_lift_destination(_lift_name, _destination, true); + // In the context, save the fact that the lift has already arrived for this + // destination so we can short-circuit the usual event-driven logic. + _context->_set_lift_arrived(_lift_name, _destination); + // 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()) @@ -484,7 +507,7 @@ RequestLift::PendingPhase::PendingPhase( _data(std::move(data)) { std::ostringstream oss; - oss << "Requesting lift \"" << lift_name << "\" to \"" << destination << "\""; + oss << "Requesting lift \"" << _lift_name << "\" to \"" << _destination << "\""; _description = oss.str(); } From 579c146a2ee4042cc8743885b680a3ad1dd9a037 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 11 Nov 2024 17:15:21 +0800 Subject: [PATCH 2/3] Remove debug outputs Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/events/ExecutePlan.cpp | 10 -------- .../rmf_fleet_adapter/events/GoToPlace.cpp | 4 ---- .../events/LegacyPhaseShim.cpp | 5 ---- .../rmf_fleet_adapter/phases/RequestLift.cpp | 23 ++----------------- 4 files changed, 2 insertions(+), 40 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 0a03d945f..83b5cac03 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -698,11 +698,6 @@ std::optional ExecutePlan::make( if (plan.get_waypoints().empty()) return std::nullopt; - std::stringstream ss; - ss << "Executing plan for " << context->requester_id() - << print_plan_waypoints(plan.get_waypoints(), context->navigation_graph()); - std::cout << ss.str() << std::endl; - auto plan_id = std::make_shared(recommended_plan_id); const auto& graph = context->navigation_graph(); LegacyPhases legacy_phases; @@ -1333,11 +1328,6 @@ std::optional ExecutePlan::make( rmf_task_sequence::events::Bundle::Type::Sequence, standbys, state, std::move(update))->begin([]() {}, std::move(finished)); - std::stringstream seq; - seq << "Event sequence for " << context->requester_id(); - print_events(seq, sequence->state(), 0); - std::cout << seq.str() << std::endl; - return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 39e42d679..846945bc1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -671,10 +671,6 @@ void GoToPlace::Active::_execute_plan( .name_or_index().c_str(), _context->requester_id().c_str()); - if (_execution.has_value()) - { - std::cout << " >>>>>> Replacing execution for " << _context->requester_id() << std::endl; - } _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(goal), std::move(full_itinerary), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp index 288952e91..d95c99905 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp @@ -153,11 +153,6 @@ auto LegacyPhaseShim::Active::make( { if (self->_finished) { - rmf_task::VersionedString::Reader reader; - std::cout << " !!!!!!!!!!!!!!!!!!!! TRIGGERING LEGACY FINISHED FOR [" - << self->_state << "] " - << *reader.read(self->_state->name()) << ": " << *reader.read(self->_state->detail()) - << std::endl; // We don't change the event status here because that should have been // done in the task summary update, and from here we don't know what // kind of finish the legacy phase may have had (e.g. completed, 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 365ce25f3..090cc1621 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -97,9 +97,6 @@ void RequestLift::ActivePhase::_init_obs() { using rmf_lift_msgs::msg::LiftState; - std::cout << " >>> INITIALIZING RequestLift for " << _context->requester_id() - << ": " << _description << std::endl; - if (_data.located == Located::Inside) { // If the robot is requesting from the inside, then we should update the @@ -225,10 +222,7 @@ void RequestLift::ActivePhase::_init_obs() } return true; })) - .take_until(_cancelled.get_observable().filter([c = _context](auto b) { - std::cout << " ================= CANCELLED TRIGGERED FOR " << c->requester_id() << ": " << b << std::endl; - return b; - })) + .take_until(_cancelled.get_observable().filter([c = _context](auto b) { return b; })) .concat(rxcpp::observable<>::create( [weak = weak_from_this()](const auto& s) { @@ -244,10 +238,6 @@ void RequestLift::ActivePhase::_init_obs() { if (const auto me = weak.lock()) { - std::cout << "Calling RequestLift finish for " - << me->_context->requester_id() - << " | " << me->_description - << ": " << __LINE__ << std::endl; if (!me->_finish()) { return; @@ -291,10 +281,6 @@ void RequestLift::ActivePhase::_init_obs() "process is finished.", me->_context->requester_id().c_str()); - std::cout << "Calling RequestLift finish for " - << me->_context->requester_id() - << " | " << me->_description - << ": " << __LINE__ << std::endl; if (me->_finish()) s.on_completed(); }); @@ -302,10 +288,6 @@ void RequestLift::ActivePhase::_init_obs() } } - std::cout << "Calling RequestLift finish for " - << me->_context->requester_id() - << " | " << me->_description - << ": " << __LINE__ << std::endl; if (me->_finish()) s.on_completed(); })); @@ -445,7 +427,6 @@ void RequestLift::ActivePhase::_do_publish() //============================================================================== bool RequestLift::ActivePhase::_finish() { - std::cout << "RequestLift finished has been triggered for " << _context->requester_id() << std::endl; // 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 @@ -507,7 +488,7 @@ RequestLift::PendingPhase::PendingPhase( _data(std::move(data)) { std::ostringstream oss; - oss << "Requesting lift \"" << _lift_name << "\" to \"" << _destination << "\""; + oss << "Requesting lift [" << _lift_name << "] to [" << _destination << "]"; _description = oss.str(); } From f4bfbc0df860a0078d7b26625fec48f622ac3819 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 11 Nov 2024 17:31:24 +0800 Subject: [PATCH 3/3] Revert unnecessary capture Signed-off-by: Michael X. Grey --- 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 090cc1621..803670b3c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -222,7 +222,7 @@ void RequestLift::ActivePhase::_init_obs() } return true; })) - .take_until(_cancelled.get_observable().filter([c = _context](auto b) { return b; })) + .take_until(_cancelled.get_observable().filter([](auto b) { return b; })) .concat(rxcpp::observable<>::create( [weak = weak_from_this()](const auto& s) {