Merge pull request #114 from AutoModality/AM-631/super-responds-to-error

Am 631/super responds to lifecycle error

Author: Dillon-Roller

include/am_super/super_node_mediator.h

@@ -72,6 +72,9 @@ class SuperNodeMediator
 
     /** Last state of the controller received */
     ControllerState last_controller_state_received;
+
+    /** Signals if any of the manifested nodes status errored */
+    bool status_error = false;
   };
 
   /**Encapsulates properties and methods that relate to the transition of states
@@ -183,6 +186,9 @@ class SuperNodeMediator
    */
   StateTransition getStateTransition(const Supervisor &supervisor);
 
+  /** returns a state transition that should be attempted when a LifeCycleError has occured. */
+  StateTransition getErrorTransition();
+
   /**
    * @return Default StateTransition with all of its fields at default. A transition is valid if it's values aren't default
    */

rostest/error_status/error_status_rostest.cpp

@@ -18,6 +18,7 @@ TEST_F(LifeCycleErrorTest, testStatus_Error)
   //LifeCycle State and Status should go error for this node
   waitUntilStatus(LifeCycleStatus::ERROR);
   waitUntil(LifeCycleState::ERROR_PROCESSING);
+  waitUntilMissionState(brain_box_msgs::VxState::SHUTDOWN);
 }
 
 int main(int argc, char** argv)

src/am_super/am_super.cpp

@@ -342,6 +342,10 @@ class AMSuper : AMLifeCycle
         ROS_INFO_STREAM(node_name << " changed status to = " << life_cycle_mediator_.statusToString(status));
         nr.status = status;
         nodes_changed = true;
+        if(status == LifeCycleStatus::ERROR)
+        {
+          supervisor_.status_error = true;
+        }
       }
       if (nr.pid != pid)
       {
@@ -567,8 +571,9 @@ class AMSuper : AMLifeCycle
     }
     else
     {
-      // ask the mediator to check with the supervisor
+      
       SuperNodeMediator::TransitionInstructions transition_instructions = node_mediator_.transitionReady(supervisor_);
+
       if (transition_instructions.ready_for_transition)
       {
         setSystemState(transition_instructions.new_state);

src/am_super/super_node_mediator.cpp

@@ -124,6 +124,11 @@ SuperNodeMediator::StateTransition SuperNodeMediator::getStateTransition(const S
   return invalidTransition();
 }
 
+SuperNodeMediator::StateTransition SuperNodeMediator::getErrorTransition()
+{
+  return {SuperState::SHUTDOWN, SuperNodeMediator::checkNodesShuttingDownOrFinalized, LifeCycleCommand::SHUTDOWN};
+}
+
 SuperNodeMediator::StateTransition SuperNodeMediator::invalidTransition()
 {
   return StateTransition();
@@ -143,8 +148,17 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup
 
   // only check those states registered with state_transitions
   if (state_transitions_.count(supervisor.system_state))
-  {
-    StateTransition transition = getStateTransition(supervisor);
+  { 
+    StateTransition transition;
+
+    if(supervisor.status_error)
+    {
+      transition = getErrorTransition();
+    }
+    else
+    {
+      transition = getStateTransition(supervisor);
+    }
     // each state has a check method providing the logic that should cause transition (based on manifest nodes
     // lifecycle)
     // some transitions happen only when check fails (mostly to abort)

test/transition_ready_unit_tests.cpp

@@ -34,13 +34,15 @@ class TransitionReady : public ::testing::Test
                               SuperNodeMediator::SuperFltCtrlState flt_ctrl_state, bool expected_ready,
                               SuperState expected_state, bool expected_resend_life_cycle_command,
                               LifeCycleCommand life_cycle_command, OperatorCommand last_op_command_received = OperatorCommand::ARM,
-                              ControllerState last_controller_state_received = SuperNodeMediator::StateTransition::NO_CONTROLLER_STATE)
+                              ControllerState last_controller_state_received = SuperNodeMediator::StateTransition::NO_CONTROLLER_STATE,
+                              bool status_error = false)
   {
     SuperNodeMediator::Supervisor supervisor;
     supervisor.system_state = from;
     supervisor.flt_ctrl_state = flt_ctrl_state;
     supervisor.last_op_command_received = last_op_command_received;
     supervisor.last_controller_state_received = last_controller_state_received;
+    supervisor.status_error = status_error;
 
     {
       SuperNodeMediator::SuperNodeInfo node = manifested_online_node_fixture();
@@ -215,6 +217,24 @@ TEST_F(TransitionReady, TransitionReady_AbortToDisarmingOnControllerStateComplet
                           ControllerState::COMPLETED);
 }
 
+TEST_F(TransitionReady, TransitionReady_AnyStateToShutdownOnStatusError_AllNodesShuttingDownOrFinalized)
+{
+  ASSERT_TRANSITION_READY(superNodeMediator, SuperState::BOOTING, LifeCycleState::FINALIZED, 
+                        (SuperNodeMediator::SuperFltCtrlState)NULL, true, SuperState::SHUTDOWN, 
+                          false, (LifeCycleCommand)NULL, SuperNodeMediator::StateTransition::NO_OPERATOR_COMMAND, 
+                          SuperNodeMediator::StateTransition::NO_CONTROLLER_STATE, true);
+
+  ASSERT_TRANSITION_READY(superNodeMediator, SuperState::READY, LifeCycleState::SHUTTING_DOWN, 
+                        (SuperNodeMediator::SuperFltCtrlState)NULL, true, SuperState::SHUTDOWN, 
+                          false, (LifeCycleCommand)NULL, SuperNodeMediator::StateTransition::NO_OPERATOR_COMMAND, 
+                          SuperNodeMediator::StateTransition::NO_CONTROLLER_STATE, true);
+
+  ASSERT_TRANSITION_READY(superNodeMediator, SuperState::ARMED, LifeCycleState::SHUTTING_DOWN, 
+                        (SuperNodeMediator::SuperFltCtrlState)NULL, true, SuperState::SHUTDOWN, 
+                          false, (LifeCycleCommand)NULL, SuperNodeMediator::StateTransition::NO_OPERATOR_COMMAND, 
+                          SuperNodeMediator::StateTransition::NO_CONTROLLER_STATE, true);
+}
+
 TEST_F(TransitionReady, DISABLED_transitionReady_AutoToAbortWhenDeactivated)
 {
   ASSERT_TRANSITION_READY(superNodeMediator, SuperState::AUTO, LifeCycleState::INACTIVE, true, SuperState::ABORT);