[Feature request] Go into TimeOut state if no pose updates arrive (for a configurable amount of time)

[chhtz]

It can happen that the pose provider fails and the task which takes the motion_command keeps using the last command. It might be an option to time-out the trajectory follower, if no pose updates arrive (would be relatively easy to implement).

On the other hand, this could also be burdened to the task which receives the motion_command, i.e., as long the trajectory follower only sends new motion commands when it gets pose updates, the following task should time out (this would need to be implemented for every robot, and may make it inconvenient, e.g. to manually send a motion command once to have a robot drive straight or in circles).

Any opinions on that @rock-control?

[chhtz]

Another alternative would be to have the pose-provider send NaN-Poses if it is not able to get the current pose (e.g., the vicon driver supports that), but I think last time I checked, the trajectory follower crashed or failed in some other way when that happened (need to check, if that was fixed).

[planthaber]

I think an error state "NO_POSITION_UPDATES" could be implemented that a higher level can react or at least shows the issue in rock-display (just like "FINISHED_TRAJECTORIES"). Just like the STABILITY_FAILED error state it coudl be self-recovering in case poses are received again.

This state could be set whenever the transformer times out, so we don't need an additional parameter

[chhtz]

If we can make it work with existing parameters, I'm fine. Probably we should still have a parameter which decides whether to send a zero-command if we don't get position updates (I guess this would always make sense, but I'm a bit afraid that we break some existing behavior if we do).

[skasperski]

I think sending a zero-command when the trajectory_follower cannot get the robot pose anymore should not interfere with any desired behavior. And as Steffen said, it would be much less intrusive then having the controllers need to timeout on incoming motion commands.

[Rauldg]

Are the motion commands from the trajectory follower old? I thought after no poses are provided it keeps using the old pose and trying to follow without perceiving motion. If the commands are new then I think the one to blame is the trajectory follower. On the other hand, if the motion commands are old, why is the motion controller still executing them? Shouldn't a safe control loop expect commands on every cycle or at least reject very old ones?

[Rauldg]

Seems like the trajectory follower keeps sending new commands even if it does not get new poses.

[Rauldg]

Thus, I am also in favor with most of what was proposed:

• Adding a new state "NO_POSITION_UPDATES" as @planthaber proposes
• Adding a parameter "stop_on_no_poses" as @chhtz proposes

if "stop_on_no_poses" is True -> trajectory follower sends stop commands while poses are not available

if "stop_on_no_poses" is False -> trajectory follower does not send any commands while poses are not available

[chhtz]

WIP: https://github.com/rock-control/control-orogen-trajectory_follower/pull/14