From e21d5a13e35db3c74e4ea80a1ab71c7c61dea5eb Mon Sep 17 00:00:00 2001 From: Emanuele Date: Mon, 4 Nov 2019 14:29:47 +0100 Subject: [PATCH] Primo tentativo di Override della gestione segnali shutdown --- .../include/roboglue_ros/roboglue_utils.h | 1 + src/roboglue_ros/src/roboglue_follower.cpp | 17 ++++++++++++++--- src/roboglue_ros/src/roboglue_recorder.cpp | 8 +++++--- src/roboglue_ros/src/roboglue_relay.cpp | 5 ++++- src/roboglue_ros/src/roboglue_utils.cpp | 10 +++++++++- 5 files changed, 33 insertions(+), 8 deletions(-) diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index cf6e7a0..578c46d 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -3,6 +3,7 @@ #endif // ROBOGLUE_UTILS_H +#include #include #include #include diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 0458cf8..252a483 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -16,13 +16,17 @@ #include /// utility function library #include - //////////////////////////////////////////// ///////// FUNCTION DECLARATIONS /////////// ////////////////////////////////////////// void testFunction( ros::Publisher* trj_pub); trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos, std::vector 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; } //////////////////////////////////////// diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 28c1f60..0bf3882 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/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; } //////////////////////////////////////// diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 161284e..27ab7ab 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/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; } diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index d867319..3ef49c8 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -1,7 +1,10 @@ #include /// 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;