Error State shouldn't be a trap

[mpetersen94]

When Spartan detected a bad command from a plan, the running plan was ended with a status about safety checks and the robot was told to hold position (go to the idle state).

The current workflow will transition to the error state when a bad command comes from the plan. The plan manager gets stuck in this state and will not publish commands until the plan manager is restarted.

We should make the error handling more graceful and only use the error state for truly unrecoverable errors.

[hjsuh94]

Could I summarize this as - do we wanna hear the brakes click when we go into error state? :)

I wonder if there are any use cases where we don't want the error state to be a trap state - the conceivable cases where error occurs (near singularities, hitting force guards, etc.) are mostly cases where we need to shut down plan runner and manually teleop the robot out of a bad configuration.

I do think that bad plan = brakes on would be a nice extra cautious behavior.

[mpetersen94]

I think a better summary is, do we want to have to reboot plan runner when an obviously bad plan is sent to it?

When a plan's step function returns a nan or when the next command is too far away from the current state, we know that is bad. Just throwing out that plan and holding for the next plan makes a lot of sense to me.

[mpetersen94]

I'm bumping this in response to our discussion yesterday. I think that the Error State should be self healing to the extent that this makes sense to repair itself programmatically. Addressing this will also help address #17. I think the big changes that should be implemented are:

• Change PlanManagerStateBase::CommandHasError() so that when it detects NaN's or that the command is too far from the current position, it logs what it detects, pops that plan from the queue and goes to the Idle state.
• Have the Error state only be triggered when a problem occurs closer to the hardware (e.g. the driver dies, the robot hits a safety, etc.) In this case, the plan runner should wait for the problem to be fixed and then switch to the Init or Idle state when it detects that.