From 96f165a361b7604f3c682839f9ccc575635f5c3a Mon Sep 17 00:00:00 2001 From: ignaciotb Date: Tue, 5 Dec 2023 16:43:04 +0100 Subject: [PATCH] Emergency manager tested in sim --- sam_action_servers/CMakeLists.txt | 6 +- .../config/emergency_config.yaml | 32 +++ sam_action_servers/launch/sam_actions.launch | 15 +- sam_action_servers/src/emergency_manager.cpp | 191 ++++++++++++++++-- 4 files changed, 225 insertions(+), 19 deletions(-) create mode 100644 sam_action_servers/config/emergency_config.yaml diff --git a/sam_action_servers/CMakeLists.txt b/sam_action_servers/CMakeLists.txt index cd00f84..fadbea5 100644 --- a/sam_action_servers/CMakeLists.txt +++ b/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 ) ############# diff --git a/sam_action_servers/config/emergency_config.yaml b/sam_action_servers/config/emergency_config.yaml new file mode 100644 index 0000000..29765ab --- /dev/null +++ b/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 \ No newline at end of file diff --git a/sam_action_servers/launch/sam_actions.launch b/sam_action_servers/launch/sam_actions.launch index 24626c4..b81ac72 100644 --- a/sam_action_servers/launch/sam_actions.launch +++ b/sam_action_servers/launch/sam_actions.launch @@ -1,16 +1,19 @@ + - - - - - - + + + + + + + + diff --git a/sam_action_servers/src/emergency_manager.cpp b/sam_action_servers/src/emergency_manager.cpp index cd80c60..b4788fb 100644 --- a/sam_action_servers/src/emergency_manager.cpp +++ b/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 #include "ros_type_introspection/ros_introspection.hpp" #include +#include +#include +#include +#include +#include + #include +#include +#include using namespace RosIntrospection; using topic_tools::ShapeShifter; +class EmergencyServer +{ + +public: + + ros::NodeHandle* nh_; + actionlib::SimpleActionServer* 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(("thruster_1_top"), thruster_1_top, "core/thruster_1_cmd"); + nh_->param(("thruster_2_top"), thruster_2_top, "core/thruster_2_cmd"); + nh_->param(("vbs_cmd_top"), vbs_cmd_top, "core/thruster_2_cmd"); + thrust_1_cmd_pub_ = nh_->advertise(thruster_1_top, 1); + thrust_2_cmd_pub_ = nh_->advertise(thruster_2_top, 1); + vbs_cmd_pub_ = nh_->advertise(vbs_cmd_top, 1); + + std::string emergency_as; + nh_->param(("emergency_as"), emergency_as, "emergency_as"); + as_ = new actionlib::SimpleActionServer(*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 *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(("abort_top"), abort_top, "/sam/core/abort"); + abort_pub_ = nh_->advertise(abort_top, 10); + + std::string thruster_1_top, thruster_2_top, vbs_cmd_top; + nh_->param(("thruster_1_top"), thruster_1_top, "core/thruster_1_cmd"); + nh_->param(("thruster_2_top"), thruster_2_top, "core/thruster_2_cmd"); + nh_->param(("vbs_cmd_top"), vbs_cmd_top, "core/thruster_2_cmd"); + thrust_1_cmd_pub_ = nh_->advertise(thruster_1_top, 1); + thrust_2_cmd_pub_ = nh_->advertise(thruster_2_top, 1); + vbs_cmd_pub_ = nh_->advertise(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 monitor(new GenericSensorMonitor(topic, 10.)); - std::string topic2 = "/bool/test2"; - std::shared_ptr monitor2(new GenericSensorMonitor(topic2, 100.)); + ros::init(argn, args, "emergency_management"); + ros::NodeHandle nh("~"); + + // Emergency surface action server + std::shared_ptr emergency_server(new EmergencyServer(nh)); + + // Basic topics monitors + std::string emerg_config; + nh.param(("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(); + const YAML::Node &monitor = config["monitor"]; + std::vector> 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(); + double freq = value["freq"].as(); + 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();