Emergency manager tested in sim

sam_action_servers/CMakeLists.txt

@@ -27,7 +27,8 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED COMPONENTS filesystem)
+find_package(yaml-cpp)
 
 
 ## Uncomment this if the package has a setup.py. This macro ensures
@@ -131,8 +132,10 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${yaml_cpp_INCLUDE_DIRS}
 )
 
+
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
 #   src/${PROJECT_NAME}/bezier_planner.cpp
@@ -163,6 +166,7 @@ ${catkin_EXPORTED_TARGETS})
 ## Specify libraries to link a library or executable target against
 target_link_libraries(emergency_manager_node
   ${catkin_LIBRARIES}
+  yaml-cpp
 )
 
 #############

sam_action_servers/config/emergency_config.yaml

@@ -0,0 +1,32 @@
+namespace: sam
+monitor:
+  - topic:
+      name: core/dvl_raw
+      freq: 0.1 # If the DVL is gone for more than 10 sec, abort mission
+  - topic:
+      name: core/imu
+      freq: 20.
+  - topic:
+      name: core/sbg_imu
+      freq: 50.
+  - topic:
+      name: core/heartbeat
+      freq: 3.
+  - topic:
+      name: core/leak
+      freq: 4.
+  - topic:
+      name: core/vbs_fb
+      freq: 5.
+  - topic:
+      name: core/thruster1_fb
+      freq: 10.
+  - topic:
+      name: core/thruster2_fb
+      freq: 10.
+  - topic:
+      name: core/depth20_pressure
+      freq: 10.
+  - topic:
+      name: uwcomms/heartbeat
+      freq: 0.1

sam_action_servers/launch/sam_actions.launch

@@ -1,16 +1,19 @@
 <launch>
 
     <arg name="namespace" default="sam"/>
+	<arg name="emergency_config_file" default="$(find sam_action_servers)/config/emergency_config.yaml"/>
 
     <group ns="$(arg namespace)">
     	<group ns="ctrl">
 			<!-- Emergency surface action -->
-			<node name="emergency_surface_action" pkg="sam_action_servers" type="emergency_surface_action.py" output="screen">
-				<param name="emergency_topic" value="/$(arg namespace)/core/abort" />
-				<param name="vbs_cmd_topic" value="/$(arg namespace)/core/vbs_cmd" />
-				<param name="rpm1_cmd_topic" value="/$(arg namespace)/core/thruster1_cmd" />
-				<param name="rpm2_cmd_topic" value="/$(arg namespace)/core/thruster2_cmd" />
-				<param name="vbs_pid_enable_top" value="/$(arg namespace)/ctrl/vbs/pid_enable" />
+			<node name="emergency_manager_node" pkg="sam_action_servers" type="emergency_manager_node" output="screen">
+				<rosparam file="$(arg emergency_config_file)" command="load"/>
+				<param name="emergency_config_file" value="$(arg emergency_config_file)"/>
+				<param name="emergency_as" value="/$(arg namespace)/ctrl/emergency_surface_action" />
+				<param name="abort_top" value="/$(arg namespace)/core/abort" />
+				<param name="thruster_1_top" value="/$(arg namespace)/core/thruster1_cmd" />
+				<param name="thruster_2_top" value="/$(arg namespace)/core/thruster2_cmd" />
+				<param name="vbs_cmd_top" value="/$(arg namespace)/core/vbs_cmd" />
 			</node>
 
 		<node name="goto_waypoint" pkg="sam_action_servers" type="wp_goto_action.py" output="screen">

sam_action_servers/src/emergency_manager.cpp

@@ -4,14 +4,89 @@
 #include "sensor_msgs/Image.h"
 #include "sensor_msgs/LaserScan.h"
 #include "geometry_msgs/TwistStamped.h"
-
+#include <std_msgs/Empty.h>
 #include "ros_type_introspection/ros_introspection.hpp"
 #include <topic_tools/shape_shifter.h>
+#include <actionlib/server/simple_action_server.h>
+#include <actionlib/client/simple_action_client.h>
+#include <smarc_bt/GotoWaypointAction.h>
+#include <smarc_msgs/ThrusterRPM.h>
+#include <sam_msgs/PercentStamped.h>
+
 #include <thread>
+#include <boost/filesystem.hpp>
+#include <yaml-cpp/yaml.h>
 
 using namespace RosIntrospection;
 using topic_tools::ShapeShifter;
 
+class EmergencyServer
+{
+
+public:
+
+    ros::NodeHandle* nh_;
+    actionlib::SimpleActionServer<smarc_bt::GotoWaypointAction>* as_;
+    smarc_bt::GotoWaypointFeedback as_fb_;
+    smarc_bt::GotoWaypointResult as_result_;
+    ros::Publisher thrust_1_cmd_pub_, thrust_2_cmd_pub_, vbs_cmd_pub_;
+
+    EmergencyServer(ros::NodeHandle nh): nh_(&nh)
+    {
+        std::string thruster_1_top, thruster_2_top, vbs_cmd_top;
+        nh_->param<std::string>(("thruster_1_top"), thruster_1_top, "core/thruster_1_cmd");
+        nh_->param<std::string>(("thruster_2_top"), thruster_2_top, "core/thruster_2_cmd");
+        nh_->param<std::string>(("vbs_cmd_top"), vbs_cmd_top, "core/thruster_2_cmd");
+        thrust_1_cmd_pub_ = nh_->advertise<smarc_msgs::ThrusterRPM>(thruster_1_top, 1);
+        thrust_2_cmd_pub_ = nh_->advertise<smarc_msgs::ThrusterRPM>(thruster_2_top, 1);
+        vbs_cmd_pub_ = nh_->advertise<sam_msgs::PercentStamped>(vbs_cmd_top, 1);
+
+        std::string emergency_as;
+        nh_->param<std::string>(("emergency_as"), emergency_as, "emergency_as");
+        as_ = new actionlib::SimpleActionServer<smarc_bt::GotoWaypointAction>(*nh_, emergency_as,
+                                                                              boost::bind(&EmergencyServer::emergencyCB, this, _1),
+                                                                              false);
+        as_->start();
+    }
+
+    void emergencyCB(const smarc_bt::GotoWaypointGoalConstPtr &goal)
+    {
+        ROS_ERROR_STREAM_NAMED("Emergency manager ", "Emergency CB received");
+        ros::Time start_t = ros::Time::now();
+        ros::Duration backup_duration(3.0);
+
+        smarc_msgs::ThrusterRPM rpm_msg;
+        sam_msgs::PercentStamped vbs_msg;
+        vbs_msg.value = 0.;
+        ros::Rate r(10.);
+
+        while(!as_->isPreemptRequested() && ros::ok())
+        {
+            if (ros::Time::now() < start_t + backup_duration)
+            {
+                rpm_msg.rpm = -500;
+                thrust_1_cmd_pub_.publish(rpm_msg);
+                thrust_2_cmd_pub_.publish(rpm_msg);
+            }
+            else
+            {
+                // TODO: disable thrusters controllers here
+                rpm_msg.rpm = 0;
+                thrust_1_cmd_pub_.publish(rpm_msg);
+                thrust_2_cmd_pub_.publish(rpm_msg);
+            }
+            // TODO: disable VBS controller here
+            vbs_msg.header.stamp = ros::Time::now();
+            vbs_cmd_pub_.publish(vbs_msg);
+            r.sleep();
+        }
+        ROS_INFO_STREAM("Emergency preempted ");
+        // set the action state to preempted
+        as_->setPreempted();
+    }
+
+};
+
 class GenericSensorMonitor
 {
 
@@ -21,12 +96,39 @@ class GenericSensorMonitor
     double rate_;
     ros::CallbackQueue queue;
     ros::Subscriber subscriber_;
+    ros::Publisher abort_pub_;
+    actionlib::SimpleActionClient<smarc_bt::GotoWaypointAction> *ac_;
+    ros::NodeHandle *nh_;
+    ros::Publisher thrust_1_cmd_pub_, thrust_2_cmd_pub_, vbs_cmd_pub_;
 
     GenericSensorMonitor(std::string &topic, double rate): topic_name_(topic), rate_(rate)
     {
-        ros::NodeHandle nh;
-        nh.setCallbackQueue(&queue);
-        subscriber_ = nh.subscribe(topic_name_, 10, &GenericSensorMonitor::topicCB, this);
+        nh_ = new ros::NodeHandle("~");
+        std::string abort_top, emergency_as;
+        nh_->param<std::string>(("abort_top"), abort_top, "/sam/core/abort");
+        abort_pub_ = nh_->advertise<std_msgs::Empty>(abort_top, 10);
+
+        std::string thruster_1_top, thruster_2_top, vbs_cmd_top;
+        nh_->param<std::string>(("thruster_1_top"), thruster_1_top, "core/thruster_1_cmd");
+        nh_->param<std::string>(("thruster_2_top"), thruster_2_top, "core/thruster_2_cmd");
+        nh_->param<std::string>(("vbs_cmd_top"), vbs_cmd_top, "core/thruster_2_cmd");
+        thrust_1_cmd_pub_ = nh_->advertise<smarc_msgs::ThrusterRPM>(thruster_1_top, 1);
+        thrust_2_cmd_pub_ = nh_->advertise<smarc_msgs::ThrusterRPM>(thruster_2_top, 1);
+        vbs_cmd_pub_ = nh_->advertise<sam_msgs::PercentStamped>(vbs_cmd_top, 1);
+
+        nh_->setCallbackQueue(&queue);
+        subscriber_ = nh_->subscribe(topic_name_, 10, &GenericSensorMonitor::topicCB, this);
+
+        // Get last word of the topic name. It'll be used to find the heartbeat one
+        std::string delimiter = "/";
+        size_t pos = 0;
+        std::string token;
+        while ((pos = topic_name_.find(delimiter)) != std::string::npos)
+        {
+            token = topic_name_.substr(0, pos);
+            topic_name_.erase(0, pos + delimiter.length());
+        }
+
         std::thread(&GenericSensorMonitor::CheckQueue, this).detach();
     }
 
@@ -39,21 +141,59 @@ class GenericSensorMonitor
             if(!queue.isEmpty())
             {
                 queue.callOne(ros::WallDuration(0.01));
-                ROS_INFO_STREAM("Callback " << topic_name_);
                 queue_init = true;
             }
             else
             {
                 if(queue_init)
                 {
-                    ROS_ERROR_STREAM("Data not coming in " << topic_name_);
+                    ROS_ERROR_STREAM_NAMED("Emergency manager: ", "data not coming in " << topic_name_ + " aborting mission");
+                    std_msgs::Empty abort;
+                    abort_pub_.publish(abort);
+                    if (topic_name_ == "heartbeat")
+                    {
+                        ROS_ERROR_STREAM_NAMED("Emergency manager ", "BT is down, aborting mission");
+                        this->emergency_no_bt();
+                    }
+                    break;
                 }
-                // break;
             }
             r.sleep();
         }    
     }
 
+    void emergency_no_bt()
+    {
+        ros::Time start_t = ros::Time::now();
+        ros::Duration backup_duration(1.0);
+
+        smarc_msgs::ThrusterRPM rpm_msg;
+        sam_msgs::PercentStamped vbs_msg;
+        vbs_msg.value = 0.;
+        ros::Rate r(10.);
+
+        while (ros::ok())
+        {
+            if (ros::Time::now() < start_t + backup_duration)
+            {
+                rpm_msg.rpm = -500;
+                thrust_1_cmd_pub_.publish(rpm_msg);
+                thrust_2_cmd_pub_.publish(rpm_msg);
+            }
+            else
+            {
+                // TODO: disable thrusters controllers here
+                rpm_msg.rpm = 0;
+                thrust_1_cmd_pub_.publish(rpm_msg);
+                thrust_2_cmd_pub_.publish(rpm_msg);
+            }
+            // TODO: disable VBS controller here
+            vbs_msg.header.stamp = ros::Time::now();
+            vbs_cmd_pub_.publish(vbs_msg);
+            r.sleep();
+        }
+    }
+
     // Callbacks
     void topicCB(const ShapeShifter::ConstPtr& msg)
     {
@@ -64,11 +204,38 @@ class GenericSensorMonitor
 
 int main(int argn, char* args[])
 {
-    ros::init(argn, args, "arm_to_fpga");
-    std::string topic = "/bool/test";
-    std::shared_ptr<GenericSensorMonitor> monitor(new GenericSensorMonitor(topic, 10.));
-    std::string topic2 = "/bool/test2";
-    std::shared_ptr<GenericSensorMonitor> monitor2(new GenericSensorMonitor(topic2, 100.));
+    ros::init(argn, args, "emergency_management");
+    ros::NodeHandle nh("~");
+
+    // Emergency surface action server
+    std::shared_ptr<EmergencyServer> emergency_server(new EmergencyServer(nh));
+
+    // Basic topics monitors
+    std::string emerg_config;
+    nh.param<std::string>(("emergency_config_file"), emerg_config, "emergency_config.yaml");
+    boost::filesystem::path emerg_config_path(emerg_config);
+
+    if (!boost::filesystem::exists(emerg_config_path))
+    {
+        ROS_ERROR_STREAM_NAMED("Emergency manager ", "config file doesn't exit " << emerg_config_path.string());
+        exit(0);
+    }
+
+    YAML::Node config = YAML::LoadFile(emerg_config_path.string());
+    std::string robot_name = config["namespace"].as<std::string>();
+    const YAML::Node &monitor = config["monitor"];
+    std::vector<std::shared_ptr<GenericSensorMonitor>> monitors;
+    for (YAML::const_iterator it = monitor.begin(); it != monitor.end(); ++it)
+    {
+        YAML::Node value = it->begin()->second;
+        if (value.Type() == YAML::NodeType::Map)
+        {
+            std::string topic_name = value["name"].as<std::string>();
+            double freq = value["freq"].as<double>();
+            std::string topic = "/" + robot_name + "/" + topic_name;
+            monitors.emplace_back(new GenericSensorMonitor(topic, freq * 0.9)); // Decrease slightly freq to account for slow transmission at times
+        }
+    }
 
     ros::spin();