Can Rmader replan trajectories when drones are lagging behind the target waypoints?

Hi,

I have encountered some challenges when deploying R-mader on real drones. Specifically, I wonder whether R-mader can detect the deviations of the drones from their target waypoints and subsequently adjust the plans.

In my current environment, the drones publish their positions via the rostopic /SQxxs/state, and receive position commands from R-mader through /SQxxs/goal. To eliminate interference from the simulator, I have disabled perfect_controller.

However, the drones sometimes fly slower than R-mader expects and therefore lag behind the target waypoints. In such cases, R-mader seems to disregard the actual drone positions and continues planning trajectories based on its previous commands.

I've attached an image illustrating this issue. In this scenario, three drones are supposed to be flying forward, but the drone in the middle remains stationary at the origin. R-mader, though, supposes all trajectories are completed and continues to plan forward.

What might be wrong in my setup? Any assistance would be greatly appreciated.

Start by tracing how R-mader consumes positions from /SQxxs/state and sends commands through /SQxxs/goal, with perfect_controller disabled. Compare the reported state with the planner's completion condition and determine what evidence would show that lagging drones are replanned from their actual positions.