Check safety during transition (#518)

* Check safety during transition

If the state doesn't successfully change, check the safety between each iteration and if False send to parking

Note: no test for this written

* Adding log message for failed state transitions

pocs/state/machine.py

@@ -137,11 +137,20 @@ def run(self, exit_when_done=False, run_once=False):
 
                 # If we didn't successfully transition, sleep a while then try again
                 if not state_changed:
-                    if _loop_iteration > 5:
+                    self.logger.warning("Failed to transition from {} to {}",
+                                        self.state, self.next_state)
+                    if self.is_safe() is False:
+                        self.logger.warning(
+                            "Conditions have become unsafe; setting next state to 'parking'")
+                        self.next_state = 'parking'
+                    elif _loop_iteration > 5:
                         self.logger.warning("Stuck in current state for 5 iterations, parking")
                         self.next_state = 'parking'
                     else:
                         _loop_iteration = _loop_iteration + 1
+                        self.logger.warning(
+                            "Sleeping for a bit, then trying the transition again (loop: {})",
+                            _loop_iteration)
                         self.sleep(with_status=False)
                 else:
                     _loop_iteration = 0