From 0149beeb79a10b9d80e139056163ff1c24faf08d Mon Sep 17 00:00:00 2001 From: ignaciotb Date: Thu, 11 Jan 2024 16:41:00 +0100 Subject: [PATCH] Nodes monitor tested in sim --- smarc_bt/src/bt_actions.py | 2 -- .../smarc_vehicle_monitor/node_monitor.hpp | 2 +- smarc_vehicle_monitor/src/node_monitor.cpp | 19 +++++++----- .../src/vehicle_monitor_node.cpp | 29 ++++++++++++++----- 4 files changed, 35 insertions(+), 17 deletions(-) diff --git a/smarc_bt/src/bt_actions.py b/smarc_bt/src/bt_actions.py index 222766d..4cdf8be 100644 --- a/smarc_bt/src/bt_actions.py +++ b/smarc_bt/src/bt_actions.py @@ -56,8 +56,6 @@ def update(self): return pt.Status.SUCCESS - - class A_ReadWaypoint(pt.behaviour.Behaviour): def __init__(self, ps_topic, diff --git a/smarc_vehicle_monitor/include/smarc_vehicle_monitor/node_monitor.hpp b/smarc_vehicle_monitor/include/smarc_vehicle_monitor/node_monitor.hpp index 10e0506..44fe011 100644 --- a/smarc_vehicle_monitor/include/smarc_vehicle_monitor/node_monitor.hpp +++ b/smarc_vehicle_monitor/include/smarc_vehicle_monitor/node_monitor.hpp @@ -15,7 +15,7 @@ class MonNodeMonitor ros::Subscriber subscriber_; ros::Publisher abort_pub_; ros::NodeHandle *nh_; - bool node_up_; + bool node_up_, emergency_; std::map nodes_; MonNodeMonitor(std::string topic_name, double rate, ros::Publisher& abort_pub); diff --git a/smarc_vehicle_monitor/src/node_monitor.cpp b/smarc_vehicle_monitor/src/node_monitor.cpp index ac9284e..7eb3008 100644 --- a/smarc_vehicle_monitor/src/node_monitor.cpp +++ b/smarc_vehicle_monitor/src/node_monitor.cpp @@ -9,6 +9,7 @@ MonNodeMonitor::MonNodeMonitor(std::string topic_name, double rate, ros::Publish ROS_INFO_STREAM("Node monitor: listening to " << topic_name_); subscriber_ = nh_->subscribe(topic_name_, 1, &MonNodeMonitor::MonCB, this); node_up_ = false; + emergency_ = false; std::thread(&MonNodeMonitor::MonitorNodes, this).detach(); } @@ -52,7 +53,7 @@ void MonNodeMonitor::MonCB(const rosmon_msgs::State::ConstPtr& state_msg) auto result = nodes_.emplace(node_i.ns + "/" + node_i.name, nodes_.size()); if (result.second) { - std::cout << "Node monitor: monitoring " << node_i.ns + "/" + node_i.name << std::endl; + ROS_INFO_STREAM("Node monitor: monitoring " << node_i.ns << "/" << node_i.name); } // std::cout << (result.second ? "Inserted: " : "ignored: ") << node_i.ns + "/" + node_i.name << std::endl; @@ -83,10 +84,14 @@ void MonNodeMonitor::MonCB(const rosmon_msgs::State::ConstPtr& state_msg) void MonNodeMonitor::emergency_detected(std::string name) { - ROS_ERROR("-------------------------------------------------------------------"); - ROS_ERROR_STREAM("Rosmon monitor: " << name << " crahed, aborting mission"); - ROS_ERROR("-------------------------------------------------------------------"); - std_msgs::Empty abort; - abort_pub_.publish(abort); - node_up_ = false; // And reset monitor + if(!emergency_) + { + ROS_ERROR("-------------------------------------------------------------------"); + ROS_ERROR_STREAM("Rosmon monitor: " << name << " crahed, aborting mission"); + ROS_ERROR("-------------------------------------------------------------------"); + std_msgs::Empty abort; + abort_pub_.publish(abort); + node_up_ = false; // And reset monitor + emergency_ = true; // And set emergency to true + } } \ No newline at end of file diff --git a/smarc_vehicle_monitor/src/vehicle_monitor_node.cpp b/smarc_vehicle_monitor/src/vehicle_monitor_node.cpp index 14a9f9b..28e4c06 100644 --- a/smarc_vehicle_monitor/src/vehicle_monitor_node.cpp +++ b/smarc_vehicle_monitor/src/vehicle_monitor_node.cpp @@ -14,6 +14,11 @@ #include "smarc_vehicle_monitor/sensor_monitor.hpp" #include "sam_action_servers/emergency_server.hpp" #include "smarc_vehicle_monitor/node_monitor.hpp" +#include "smarc_vehicle_monitor/tf_monitor.hpp" + +using namespace tf; +using namespace ros; +using namespace std; void handle_leak(const smarc_msgs::Leak::ConstPtr &msg, actionlib::SimpleActionClient &ac_g2wp, @@ -63,7 +68,7 @@ void handle_abort(const std_msgs::Empty::ConstPtr &msg, } -// Monitors that all relevant topics are up and running and the state of the ROS master +// Monitors that all relevant topics are up and running void vehicle_state_cb(std::vector> &monitors, ros::Publisher &vehicle_state_pub) { @@ -103,6 +108,7 @@ void system_state_cb(std::vector> &node_monitors { ros::master::getTopics(master_topics); std::string mon_topic = "state"; + // ROS_INFO("Holaaaaaaaaaaaaaaaaaaaa"); for (ros::master::V_TopicInfo::iterator it = master_topics.begin(); it != master_topics.end(); it++) { const ros::master::TopicInfo &topic_i = *it; @@ -172,19 +178,24 @@ int main(int argn, char *args[]) } } - // Handle leak or abort messages, equally and only once + // Handle abort messages only once bool emergency_trigger = false; boost::function abort_cb = [&ac_g2wp, &ac_emerg, &emergency_trigger] (const std_msgs::Empty::ConstPtr &msg) { handle_abort(msg, ac_g2wp, ac_emerg, emergency_trigger); }; + ros::Subscriber abort_subs = nh.subscribe(abort_top, 1, abort_cb); + // Handle leak messages only once boost::function leak_cb = [&ac_g2wp, &ac_emerg, &emergency_trigger, &abort_pub] (const smarc_msgs::Leak::ConstPtr &msg) { handle_leak(msg, ac_g2wp, ac_emerg, abort_pub, emergency_trigger); }; + std::string leak_top; + nh.param(("leak_top"), leak_top, "leak"); + ros::Subscriber leak_subs = nh.subscribe(leak_top, 1, leak_cb); // Monitor state of the vehicle to let the BT know when it's ready for a mission nh.param(("vehicle_ready_top"), vehicle_ready_top, "vehicle_ready"); @@ -204,6 +215,7 @@ int main(int argn, char *args[]) nh.param(("master_check_period"), master_check_period, 1.); ros::Timer master_state_timer = nh.createTimer(ros::Duration(master_check_period), master_cb); + // Monitor states of nodes std::vector> node_monitors; ros::master::V_TopicInfo master_topics; std::vector mon_topics; @@ -215,12 +227,15 @@ int main(int argn, char *args[]) nh.param(("nodes_check_period"), nodes_check_period, 2.); ros::Timer nodes_monitor_timer = nh.createTimer(ros::Duration(nodes_check_period), mon_timer_cb); - std::string leak_top; - nh.param(("leak_top"), leak_top, "leak"); - ros::Subscriber leak_subs = nh.subscribe(leak_top, 1, leak_cb); - ros::Subscriber abort_subs = nh.subscribe(abort_top, 1, abort_cb); - ros::spin(); + // TF monitor: let's the user know when the tree is fully built and raises and alarm if it hasn't been updated in 5 seconds + bool using_specific_chain = true; + std::string tf_prefix, framea, frameb; + nh.param(("utm_frame"), frameb, "utm"); + nh.param(("base_frame"), framea, "base_link"); + smarc::TFMonitor tf_monitor(abort_pub, tf::resolve(tf_prefix, framea), tf::resolve(tf_prefix, frameb)); + ros::spin(); + return 0; }