diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index a736ce9e5..c6105be1c 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -19,6 +19,7 @@ #define RMF_FLEET_ADAPTER__STANDARDNAMES_HPP #include +#include namespace rmf_fleet_adapter { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 0abfd7165..4cd94e281 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1017,6 +1017,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, @@ -1278,6 +1292,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) @@ -1286,11 +1314,29 @@ void RobotContext::_check_lift_state( { if (_lift_destination && !_lift_destination->requested_from_inside) { - // The lift destination reference count dropping to one while the lift - // destination request is on the outside means the task that prompted the - // lift usage was cancelled while the robot was outside of the lift. - // Therefore we should release the usage of the lift. - release_lift(); + 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(30)) + { + // 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. + RCLCPP_INFO( + _node->get_logger(), + "Requesting lift [%s] to be released for [%s] because it is outside " + "the lift and not holding a claim for an extended period of time.", + _lift_destination->lift_name.c_str(), + requester_id().c_str()); + release_lift(); + } + } + else + { + _initial_time_idle_outside_lift = now; + } } else if (_lift_destination && !_current_task_id.has_value()) { @@ -1315,7 +1361,7 @@ void RobotContext::_check_lift_state( if (_initial_time_idle_outside_lift.has_value()) { const auto lapse = now - *_initial_time_idle_outside_lift; - if (lapse > std::chrono::seconds(2)) + if (lapse > std::chrono::seconds(10)) { RCLCPP_INFO( _node->get_logger(), @@ -1323,8 +1369,8 @@ void RobotContext::_check_lift_state( "outside of the lift.", _lift_destination->lift_name.c_str(), requester_id().c_str()); + release_lift(); } - release_lift(); } else { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 9c82be580..0d10da30a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -655,6 +655,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, @@ -711,6 +717,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/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 362962346..7e78f5f11 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -465,9 +465,9 @@ std::optional GoToPlace::Active::_choose_goal( { RCLCPP_ERROR( _context->node()->get_logger(), - "No path found for robot [%s] to waypoint [%lu]", - wp_idx, - _context->requester_id().c_str()); + "No path found for robot [%s] to waypoint [%zu]", + _context->requester_id().c_str(), + wp_idx); } } 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 57272a2db..4dcdce096 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,12 +97,12 @@ void RequestLift::ActivePhase::_init_obs() { using rmf_lift_msgs::msg::LiftState; - 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) @@ -111,6 +111,16 @@ void RequestLift::ActivePhase::_init_obs() if (!self) return; + if (self->_data.located == Located::Outside) + { + // The robot is going to start moving into the lift now, so we + // should lock in the lift by saying that the request is coming from + // inside the lift. This will prevent the auto-detection system from + // releasing the lift prematurely. + self->_context->set_lift_destination( + self->_lift_name, self->_destination, true); + } + if (self->_data.resume_itinerary) { self->_context->schedule_itinerary( @@ -415,6 +425,10 @@ bool RequestLift::ActivePhase::_finish() // the destination in. _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()) @@ -455,7 +469,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(); }