Backport lift release fixes to Iron

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -19,6 +19,7 @@
 #define RMF_FLEET_ADAPTER__STANDARDNAMES_HPP
 
 #include <string>
+#include <cstdint>
 
 namespace rmf_fleet_adapter {
 

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<void> 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,16 +1361,16 @@ 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(),
               "Releasing lift [%s] for robot [%s] because it has remained idle "
               "outside of the lift.",
               _lift_destination->lift_name.c_str(),
               requester_id().c_str());
+            release_lift();
           }
-          release_lift();
         }
         else
         {

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<void> 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<typename... Args>
   static std::shared_ptr<RobotContext> make(Args&&... args)
   {

rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp

@@ -465,9 +465,9 @@ std::optional<rmf_traffic::agv::Plan::Goal> 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);
     }
   }
 

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<LegacyTask::StatusMsg>(
         [w = weak_from_this()](rxcpp::subscriber<LegacyTask::StatusMsg> 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();
 }