Primo tentativo di Override della gestione segnali shutdown

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -3,6 +3,7 @@
 
 #endif // ROBOGLUE_UTILS_H
 
+#include <signal.h>
 #include <iostream>
 #include <stdlib.h>
 #include <ros/ros.h>

src/roboglue_ros/src/roboglue_follower.cpp

@@ -16,13 +16,17 @@
 #include <roboglue_ros/roboglue_utils.h> /// utility function library
 #include <moveit/move_group_interface/move_group_interface.h>
 
-
 ////////////////////////////////////////////
 ///////// FUNCTION DECLARATIONS ///////////
 //////////////////////////////////////////
 void testFunction( ros::Publisher* trj_pub);
 trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos, std::vector<std::string> jnames);
 
+void sigintHandler(int sig, internalState* is){
+    ROS_DEBUG("Signal %d Received", sig);
+    if (is->isRunning) is->isRunning = false;
+    //ros::shutdown();
+}
 ///////////////////////////////////////////
 ///////// ROS CALLBACKS //////////////////
 /////////////////////////////////////////
@@ -201,8 +205,11 @@ int main(int argc, char **argv) {
 
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_follower");
+    //TODO: ros::init(argc, argv, "roboglue_follower", ros::init_options::NoSigintHandler);
     ros::NodeHandle nh;
     publisherMap publishers;
+    /// TODO: override default signal handler ///
+    // signal(SIGINT, boost::bind(sigintHandler, _1, &is));
     ///read parameters from server ///
     nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
     loadCommonParameters(&nh, &common_par);
@@ -217,7 +224,7 @@ int main(int argc, char **argv) {
     /// set spinner rate ///
     ros::Rate loop_rate(common_par.loopRate);
     /// set console log level ///
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
     ROS_INFO("Follower Node Started");
 
     /// subscribe to incoming topics ///
@@ -281,10 +288,14 @@ int main(int argc, char **argv) {
         }
         ROS_DEBUG_ONCE("Follower Node Looping");
         ros::spinOnce();
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
         loop_rate.sleep();
     }
+    ros::shutdown();
     //////////////////////////////////////////////////////
-    declareShutdown(&publishers, &common_par);
     return 0;
 }
 ////////////////////////////////////////

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -261,9 +261,8 @@ int main(int argc, char **argv) {
     nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode);
     nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode);
 
-
     /// set console log level ///
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
     /// set spinner rate ///
     ros::Rate loop_rate(common_par.loopRate);
     ROS_INFO("Recorder Node Started");
@@ -323,6 +322,10 @@ int main(int argc, char **argv) {
         }
         ROS_DEBUG_ONCE("Recorder Node Looping");
         ros::spinOnce();
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
           /////////////////////////////////////////
          /////////// PLAY FILE ///////////////////
         /////////////////////////////////////////
@@ -430,7 +433,6 @@ int main(int argc, char **argv) {
         }
         loop_rate.sleep();
     }
-    declareShutdown(&publishers, &common_par);
     return 0;
 }
 ////////////////////////////////////////

src/roboglue_ros/src/roboglue_relay.cpp

@@ -177,6 +177,10 @@ int main(int argc, char **argv) {
         }
         ROS_DEBUG_ONCE("Relay Node Looping");
         ros::spinOnce();
+        if(ros::isShuttingDown()){
+            declareShutdown(&publishers, &common_par);
+            ros::spinOnce();
+        }
         loop_rate.sleep();
     }
     ///////////////////////////////////////
@@ -187,6 +191,5 @@ int main(int argc, char **argv) {
     } catch (mqtt::exception &e) {
         ROS_DEBUG("Error Disconnecting MQTT: [%s]", e.what());
     }
-    declareShutdown(&publishers, &common_par);
     return 0;
 }

src/roboglue_ros/src/roboglue_utils.cpp

@@ -1,7 +1,10 @@
 #include <roboglue_ros/roboglue_utils.h>
 
 /// Utilities funtion used in roboglue ros nodes ///
-
+///
+////////////////////////////////////////////////////
+/////////////// NODE MANAGEMENT ///////////////////
+//////////////////////////////////////////////////
 bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) {
     nh->getParam(p->param_ns+"INstate", p->ros_topics.stateSub);
     nh->getParam(p->param_ns+"INcommands", p->ros_topics.commandSub);
@@ -32,6 +35,11 @@ void declareShutdown(publisherMap* publishers, COMMON_parameters* common_par){
     publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->shutdownMsg);
 }
 
+
+////////////////////////////////////////////////////
+/////////////// OTHER /////////////////////////////
+//////////////////////////////////////////////////
+
 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){
     static uint32_t seqId = 0;
     geometry_msgs::TwistStamped tw;