diff --git a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp index fa2e921e..5fef5a33 100644 --- a/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp +++ b/rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp @@ -396,6 +396,16 @@ bool FleetAdapterNode::handle_delay( entry.schedule->push_delay(time_difference); + const auto route_size = entry.route->trajectory().size(); + const auto remaining_path_size = state.path.size(); + if (route_size > remaining_path_size) + { + entry.schedule->participant().reached( + entry.schedule->participant().current_plan_id(), + 0, + route_size - remaining_path_size - 1); + } + // Return true to indicate that the delay has been handled. return true; }