fix: avoid status change when in error AM-695/error-methods

Author: Aaron Roller

include/super_lib/am_life_cycle.h

@@ -193,6 +193,10 @@ class AMLifeCycle
      * */
     AMStatReset& configureHzStats(AMStatReset& stats);
 
+    /** Called periodically by a timer defaulting to 1 second.
+     * Useful for checking health regularly, but not during 
+     * callbacks which can affect performance and be too granular
+     */
     virtual void heartbeatCB(const ros::TimerEvent& event);
 
     void lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg);

rostest/error_forced/error_forced_rostest.cpp

@@ -9,14 +9,20 @@ class LifeCycleErrorTest : public RostestBase, public am::AMLifeCycle
   LifeCycleErrorTest() : 
     RostestBase() 
   {}
+
+
+  void onConfigure()
+  {
+    //do nothing...stay in configure
+  }
 };
 
 TEST_F(LifeCycleErrorTest, testStatus_Error)
 {
-  waitUntil(LifeCycleState::UNCONFIGURED);
+  waitUntil(LifeCycleState::CONFIGURING,"3K3K");
   ASSERT_EQ(LifeCycleStatus::OK,getStatus());
-  error("NNAQ",true);
-  waitUntil(LifeCycleState::ERROR_PROCESSING);
+  errorTerminal("forcing error during configuration", "NNAQ");
+  waitUntil(LifeCycleState::ERROR_PROCESSING,"NAKW");
   waitUntil(LifeCycleStatus::ERROR,"IUIU");
 }
 

src/am_super/am_super.cpp

@@ -304,37 +304,37 @@ class AMSuper : AMLifeCycle
       SuperNodeMediator::SuperNodeInfo& nr = it->second;
       if (!nr.online)
       {
-        ROS_INFO_STREAM("manifested node '" << node_name << "' came online");
+        ROS_INFO_STREAM("manifested node '" << node_name << "' came online [PGPG]");
         nr.online = true;
         nodes_changed = true;
       }
       if (nr.state != state)
       {
-        ROS_INFO_STREAM(node_name << " changed state to = " << life_cycle_mediator_.stateToString(state));
+        ROS_INFO_STREAM(node_name << " changed state to = " << life_cycle_mediator_.stateToString(state) << " [38S8]");
         nr.state = state;
         nodes_changed = true;
       }
       if (nr.status != status)
       {
-        ROS_INFO_STREAM(node_name << " changed status to = " << life_cycle_mediator_.statusToString(status));
+        ROS_INFO_STREAM(node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]");
         nr.status = status;
         nodes_changed = true;
         if(nr.manifested && nr.status == LifeCycleStatus::ERROR)
         {
           supervisor_.status_error = true;
-          ROS_ERROR_STREAM("Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes..");
+          ROS_ERROR_STREAM("Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]");
         }
       }
       if (nr.pid != pid)
       {
         //process id = 0 observed to be a node coming online. -1 appears to be offline
         if(pid == 0)
         {
-          ROS_INFO_STREAM(node_name << " process is alive");
+          ROS_INFO_STREAM(node_name << " process is alive [UIRE]");
         }
         else
         {
-          ROS_WARN_STREAM(node_name << " changed process id from: " << nr.pid << " to: " <<  pid);
+          ROS_WARN_STREAM(node_name << " changed process id from: " << nr.pid << " to: " <<  pid << " [WNEW]");
         }
         nr.pid = pid;
         nodes_changed = true;

src/super_lib/am_life_cycle.cpp

@@ -103,7 +103,7 @@ void AMLifeCycle::lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr m
         configure();
         break;
       case LifeCycleCommand::CREATE:
-        ROS_WARN_STREAM("illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE));
+        ROS_WARN_STREAM("illegal command " << life_cycle_mediator_.commandToString(LifeCycleCommand::CREATE) << " [7YT8]");
         break;
       case LifeCycleCommand::DEACTIVATE:
         transition("deactivate", LifeCycleState::ACTIVE, LifeCycleState::DEACTIVATING, LifeCycleState::INACTIVE,
@@ -124,17 +124,17 @@ void AMLifeCycle::transition(std::string transition_name, LifeCycleState initial
 {
   if (life_cycle_info_.state == initial_state)
   {
-    ROS_INFO_STREAM(transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state));
+    ROS_INFO_STREAM(transition_name << ", current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [ASWU]");
     setState(transition_state);
     on_function();
   }
   else if (life_cycle_info_.state == transition_state || life_cycle_info_.state == final_state)
   {
-    ROS_DEBUG_STREAM("ignoring redundant " << transition_name);
+    ROS_DEBUG_STREAM("ignoring redundant " << transition_name << " [0393]");
   }
   else
   {
-    ROS_WARN_STREAM("received illegal " << transition_name  << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state));
+    ROS_WARN_STREAM("received illegal " << transition_name  << " in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [JGV5]");
   }
 }
 
@@ -237,12 +237,12 @@ void AMLifeCycle::destroy()
 {
   if (life_cycle_mediator_.illegalDestroy(life_cycle_info_))
   {
-    ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state));
+    ROS_INFO_STREAM("received illegal activate in state " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [45RT]");
   }
   /* This condition is hit only if state equals FINALIZED. Checking SHUTTING_DOWN is redundant */
   else
   {
-    ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state));
+    ROS_INFO_STREAM("current state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << " [RE45]");
     onDestroy();
   }
 }
@@ -281,22 +281,29 @@ void AMLifeCycle::error(std::string error_code, bool forced)
 
 void AMLifeCycle::error(std::string message, std::string error_code, bool forced)
 {
-  std::string error_code_message = " [" + error_code + "] ";
-  if (!forced && life_cycle_mediator_.redundantError(life_cycle_info_))
-  {
-    ROS_WARN_STREAM("Error called again, but previously reported." << error_code_message);
-  }
-  else if(!forced && withinConfigureTolerance())
+  std::string error_code_message = " Error[" + error_code + "] ";
+  if(withinConfigureTolerance() && !forced)
   {
-    ROS_WARN_STREAM_THROTTLE(throttle_info_.warn_throttle_s,"Ignoring" << error_code_message << " during configure tolerance of " << configure_tolerance_s << " seconds [GFRT]");
+    ROS_WARN_STREAM_THROTTLE(throttle_info_.warn_throttle_s,"Ignoring tolerant error for (" << configure_tolerance_s << "s) `" << message << "` " << error_code_message << "[GFRT]");
   }
   else
   {
-     std::string forced_prefix = forced?"Terminal ":"";
-    ROS_ERROR_STREAM(message << "  state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << error_code_message << " [R45Y]" );
-    setState(LifeCycleState::ERROR_PROCESSING);
-    setStatus(LifeCycleStatus::ERROR);
-    onError();
+    std::string forced_prefix = forced?"Terminal ":"";
+    std::string repeat_prefix = "";
+
+    //only change the state if 
+    if(!life_cycle_mediator_.redundantError(life_cycle_info_))
+    {
+      setState(LifeCycleState::ERROR_PROCESSING);
+      setStatus(LifeCycleStatus::ERROR);
+      onError();
+    }
+    else
+    {
+      repeat_prefix = "Repeated ";
+    }
+    std::string error_explanation=forced_prefix + repeat_prefix + error_code_message;
+    ROS_ERROR_STREAM(message << "  state: " << life_cycle_mediator_.stateToString(life_cycle_info_.state) << error_explanation << " [R45Y]" );
   }
 }
 
@@ -371,7 +378,11 @@ void AMLifeCycle::addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw
   }
   else
   {
-    setStatus(status);
+    //only report status if not already errored
+    if(!life_cycle_mediator_.redundantError(life_cycle_info_))
+    {
+      setStatus(status);
+    }
 
     //configuring is a special case where tolerance for errors is allowed
     if(getState() == LifeCycleState::CONFIGURING)
@@ -495,9 +506,8 @@ bool AMLifeCycle::setStatus(const LifeCycleStatus status)
   //if we are in error and want to leave it
   if(life_cycle_info_.status == LifeCycleStatus::ERROR && status != LifeCycleStatus::ERROR)
   {
-    ROS_WARN_STREAM_THROTTLE(getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status));
+    ROS_WARN_STREAM_THROTTLE(getThrottle(), "requested to change status from ERROR to " << life_cycle_mediator_.statusToString(status) << " [DFRE]");
   }
-
   else if (life_cycle_mediator_.setStatus(status, life_cycle_info_))
   {
     sendNodeUpdate();