fix: if forceTransition, then consider it a legal transition and allow super to transition

Author: dillon@dillon

include/am_super/super_node_mediator.h

@@ -281,6 +281,11 @@ class SuperNodeMediator
    */
   bool transitionIsValid(const StateTransition&);
 
+  /**
+   * @returns true - if we are allowed to transition to this state regardless of the state we are currently in
+   */
+  bool forceTransition(const SuperState& to_state);
+
 private:
   /** name of supervisor node */
   const std::string SUPER_NODE_NAME;
@@ -295,7 +300,7 @@ class SuperNodeMediator
   /** @brief temporary hack to allow manifested nodes to not halt transitions.*/
   bool lifeCycleNotYetImplemented(string node_name);
 
-  bool forceTransition(StateTransition transition);
+  
 };
 }
 

src/am_super/am_super.cpp

@@ -606,8 +606,9 @@ class AMSuper : AMLifeCycle
   {
     ROS_INFO_STREAM(state_mediator_.stateToString(supervisor_.system_state) << " --> "
                                                                             << state_mediator_.stateToString(state));
-
-    bool legal = state_mediator_.allowsTransition(supervisor_.system_state, state);
+    bool legal = true;
+    if(!node_mediator_.forceTransition(state))
+      legal = state_mediator_.allowsTransition(supervisor_.system_state, state);
 
     if (!legal)
     {

src/am_super/super_node_mediator.cpp

@@ -170,7 +170,7 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup
 
       //transition to new state if checks passed or forced
       bool checks_passed = check_results.first;
-      if (checks_passed || forceTransition(transition))
+      if (checks_passed || forceTransition(transition.to_state))
       {
         transition_instructions.ready_for_transition = true;
         transition_instructions.new_state = transition.to_state;
@@ -196,9 +196,9 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup
 }
 
 
-bool SuperNodeMediator::forceTransition(StateTransition transition)
+bool SuperNodeMediator::forceTransition(const SuperState& to_state)
 {
-  return transition.to_state == SuperState::MANUAL || transition.to_state == SuperState::SHUTDOWN;
+  return to_state == SuperState::MANUAL || to_state == SuperState::SHUTDOWN;
 }
 
 bool SuperNodeMediator::lifeCycleNotYetImplemented(string node_name)