Orunav_mpc exits abrutly.

[MFernandezCarmona]

Hi,
orunav_mpc exits with an error due to several reasons. Whenever this happens, is quite disruptive for the rest of the navigation stack (I always end up restarting the entire stack). For example, here https://github.com/OrebroUniversity/navigation_oru-release/blob/master/orunav_mpc/src/commandSender.cpp#L85

@ksatyaki, @h72a65 , I wonder if it's possible to prevent mpc from exiting or to make vehicle_execution_node able to "restart" (forgetting whatever trajectories sent to the robot and going back to initial status)

[ksatyaki]

I haven't looked deeper into the problem. But, I am guessing that the commands are sent as they are received. And if they weren't sent at the right time, it fails. Maybe it should hold a buffer and send commands from the buffer? I have no idea what happens when the 'laser-emergency' is triggered. Because the robot stop. It should recompute controls from where it left off? Just thinking out loud here.

[MFernandezCarmona]

Hi,
'laser-emergency' case is recoverable. Robot resumes after laser detection is cleared.
I'm speaking about other situations, like an illegal speed change is sent to mpc from vehicle execution node.

[h72a65]

Hi, right, as it is now the mpc controller will terminate (calling exit(-1)) if it there is some failures. Usually these are due to commands that are simply off or that it cannot compute a feasible solution (QP failed) (it has some functionality that it can reuse the last solution for some time. The reason why doing this complete stop / termination was due to safety reasons - make sure you send the right things to the controller :-). As I see it, we could relax this or make sure that the controller don't get any commands it cannot handle. For the QP failed scenario I would opt for that we allow the system to recover - that we put the system into a failure stat that we an recover from, since this is something that is very hard to predict if a QP solution exists or not before computing it. For other reasons I would opt that is better to make sure that this commands are never sent. For the vehicle execution there could be possible to add some recovery / reset / abort call and we "just" need to find out what should be done during this call given all possible states of the controller. If the vehicle is standing still is easier than if the vehicle is moving, then I suppose we shouldn't e-brake but rather de-accelerate in a nice way and then stop and so on. Best, Henrik

…

________________________________ Från: Manuel Fernandez-Carmona <notifications@github.com> Skickat: den 15 augusti 2019 13:30:29 Till: OrebroUniversity/navigation_oru-release Kopia: Henrik Andreasson; Mention Ämne: [OrebroUniversity/navigation_oru-release] Orunav_mpc exits abrutly. (#33) Hi, orunav_mpc exits with an error due to several reasons. Whenever this happens, is quite disruptive for the rest of the navigation stack (I always end up restarting the entire stack). For example, here https://github.com/OrebroUniversity/navigation_oru-release/blob/master/orunav_mpc/src/commandSender.cpp#L85 @ksatyaki<https://github.com/ksatyaki>, @h72a65<https://github.com/h72a65> , I wonder if it's possible to prevent mpc from exiting or to make vehicle_execution_node able to "restart" (forgetting whatever trajectories sent to the robot and going back to initial status) — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub<#33?email_source=notifications&email_token=ACUTFHQLM2M2UACAZOCRNC3QEU45LA5CNFSM4IL5MKW2YY3PNVWWK3TUL52HS4DFUVEXG43VMWVGG33NNVSW45C7NFSM4HFNFBZQ>, or mute the thread<https://github.com/notifications/unsubscribe-auth/ACUTFHUDWL7BG4IKHHLUTBDQEU45LANCNFSM4IL5MKWQ>.

[MFernandezCarmona]

You're right. Probably the best is let mpc to die, as something really concerning has happened and figure out how to "recover" vehicle execution node. Correct me if I'm wrong but we don't need to create any new service: ExecuteTask service has a Task as request, and Task msgs have an abort flag that means discard current task.

Let me put an example:

• Coordinator creates a new trajectory.
• MPC is not able to execute it (speed change violation ...) and dies.
• We manually relaunch MPC.
• We manually call the service "abort" from coordinator, who purgues current trajectory.
• We manually call the service "ExecuteTask" from vehicle execution node and set the flag "abort" from msg Task to true.
• [And the new code] vehicle execution node discards the task he was doing, re-checks mpc state and goes to wait state.

What do you think?