Incorporate lift disruption fixes

Signed-off-by: Michael X. Grey <[email protected]>

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)

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/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)
@@ -425,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())
@@ -465,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();
 }