Merge pull request #155 from AutoModality/AM-799/deactivate

syntax bool import AM-799/deactivate

Author: Aaron Roller

src/am_super/am_super.cpp

@@ -7,6 +7,7 @@
 #include <sensor_msgs/Joy.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/Int16.h>
+#include <std_msgs/Bool.h>
 
 #include <am_super/baby_sitter.h>
 #include <am_super/super_state.h>
@@ -68,6 +69,8 @@ class AMSuper : AMLifeCycle
   ros::Publisher system_state_pub_;
   ros::Publisher super_status_pub_;
   ros::Publisher led_pub_;
+  /** stops the flight plan when SHUTDOWN state */
+  ros::Publisher flight_plan_deactivation_pub_;
   ros::Subscriber node_state_sub_;
   ros::Subscriber operator_command_sub_;
   ros::Subscriber controller_state_sub;
@@ -199,6 +202,8 @@ class AMSuper : AMLifeCycle
      */
     super_status_pub_ = nh_.advertise<brain_box_msgs::Super2Status>(am_super_topics::SUPER_STATUS, 1000);
 
+    flight_plan_deactivation_pub_ = nh_.advertise<std_msgs::Bool>(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 1000);
+
     supervisor_.system_state = SuperState::BOOTING;
     supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT;
 
@@ -330,6 +335,7 @@ class AMSuper : AMLifeCycle
         {
           supervisor_.status_error = true;
           ROS_ERROR_STREAM("Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]");
+          stopFlightPlan();
         }
       }
       if (nr.pid != pid)
@@ -554,6 +560,16 @@ class AMSuper : AMLifeCycle
     return success;
   }
 
+
+  /** Send signal to flight controller that flight is over. */
+  void stopFlightPlan()
+  {
+    std_msgs::Bool msg;
+    msg.data = false; //false means deactivate
+    flight_plan_deactivation_pub_.publish(msg);
+    ROS_ERROR_STREAM("Sending flight plan kill command.");
+  }
+
   /**
    * check for state transition based upon current state and values of member fields.
    * Will call to modify the system state if transition is necessary. Will also call