Manage the safe mode.

[MaximilienNaveau]

Problem

The current state is that when starting the robot, zero torques are sent.
This is okay for now.
However, for other robots (think Apollo) running a PD controller / start from the SafeMode controller might be more appropriate.

Plus the transition into safe_mode is not permanent, with potential numerous switches from one control to another which is dangerous. This can happen for now when the Controller is taking too long to compute we might have controller switches.

Potential solution

Once we have the communication between the graph process and motor process, one way to do this could be to have an

• "InitMode", which is executed by the robot first,
• the dynamic graph process / user send a "run" message to the motor process and the robot starts to process the sent torque signals
• if something goes wrong, the robot transitions permanently into a SafeMode.

If the motor process is not receiving a new message for |max_missed_control| from the dynamic graph process, it will enter SafeMode.

Make the entrance into safe mode permanent with a user function to go back to init_mode.
This way if we find a way to properly re-initialize the dynamic graph or control process cleanly we do have to shutdown the robot.