Fix lift disruption issue

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp

@@ -1153,6 +1153,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,
@@ -1475,6 +1489,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)
@@ -1568,9 +1596,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)

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp

@@ -692,6 +692,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,
@@ -787,6 +793,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);
+
   /// Set an allocated destination.
   void _set_allocated_destination(
     const rmf_reservation_msgs::msg::ReservationAllocation&);

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> ExecutePlan::make(
   agv::RobotContextPtr context,

rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp

@@ -29,13 +29,29 @@ auto WaitForTraffic::Standby::make(
   const AssignIDPtr& id,
   std::function<void()> update) -> std::shared_ptr<Standby>
 {
+  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 << " <unknown>";
+    }
+  }
+  ss << " ]";
+
   auto standby = std::make_shared<Standby>();
   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);
 

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();
 }
@@ -110,12 +110,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<LegacyTask::StatusMsg>(
         [w = weak_from_this()](rxcpp::subscriber<LegacyTask::StatusMsg> s)
@@ -444,6 +444,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 +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();
 }