Aggiunto messaggio shutdown del nodo

roboglue_ros_ws.workspace.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.9.1, 2019-10-30T12:20:28. -->
+<!-- Written by QtCreator 4.9.1, 2019-11-04T11:07:27. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -124,7 +124,7 @@
      <value type="QString">ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share</value>
      <value type="QString">ROS_ROOT=/opt/ros/kinetic/share/ros</value>
      <value type="QString">ROS_VERSION=1</value>
-     <value type="QString">SHLVL=528</value>
+     <value type="QString">SHLVL=608</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -52,7 +52,7 @@ typedef struct {
     ROS_topics ros_topics;
     int loopRate, msgBufferSize;
     std::string param_ns;
-    std_msgs::String startupMsg;
+    std_msgs::String startupMsg, shutdownMsg;
 } COMMON_parameters;
 
 ///////// AUX DATA STORAGE ///////////////
@@ -109,6 +109,22 @@ typedef struct {
     double joint_jump_tresh;
 } robotData;
 
+///////// FILE DATA STORAGE /////////////
+typedef struct {
+    nlohmann::json* metaFile = nullptr;
+    rapidcsv::Document* dataFile = nullptr;
+    std::string meta_dir_name;
+    std::string data_dir_name;
+    std::string meta_template_name;
+    std::string data_template_name;
+    std::string meta_ext;
+    std::string data_ext;
+    std::vector<pointRecord>* recordVect;
+    std::vector<pointRecord>* playVect;
+} fileData;
+// posizione degli elementi nella riga del csv
+enum VPOS {X,Y,Z,RX,RY,RZ,DT,DS,VMAX};
+
 typedef struct {
     mvt::MoveItVisualToolsPtr vtools;
 } rvizData;
@@ -116,6 +132,9 @@ typedef struct {
 ///////// LOAD COMMON ROS TOPICS AND PARAMETERS /////////////////////
 bool loadCommonParameters(ros::NodeHandle*, COMMON_parameters*);
 
+void declareStartup(publisherMap*, COMMON_parameters*);
+void declareShutdown(publisherMap*, COMMON_parameters*);
+
 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json);
 trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json);
 

src/roboglue_ros/src/roboglue_follower.cpp

@@ -276,9 +276,7 @@ int main(int argc, char **argv) {
     while (ros::ok() && is.isRunning) {
         if (startCycle){
             startCycle = false;
-            common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
+            declareStartup(&publishers, &common_par);
-            ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str());
-            publishers[interface_pub.getTopic()]->publish(common_par.startupMsg);
             ros::spinOnce();
         }
         ROS_DEBUG_ONCE("Follower Node Looping");
@@ -286,6 +284,7 @@ int main(int argc, char **argv) {
         loop_rate.sleep();
     }
     //////////////////////////////////////////////////////
+    declareShutdown(&publishers, &common_par);
     return 0;
 }
 ////////////////////////////////////////

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -33,7 +33,7 @@
             che non si incappi in configurazioni instabili.
 20190708 - Implementazione funzioni di visualizzazione del percorso e pulizia dello schermo prima
             dell'esecuzione del file.
-20191029 - Spostata la documentazione suG GIT
+20191029 - Spostata la documentazione su GIT
 */
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
@@ -43,22 +43,9 @@
 #include <iostream>
 #include <fstream>
 
-typedef struct {
+////////////////////////////////////////////
-    nlohmann::json* metaFile = nullptr;
+///////// FUNCTION DECLARATIONS ///////////
-    rapidcsv::Document* dataFile = nullptr;
+//////////////////////////////////////////
-    std::string meta_dir_name;
-    std::string data_dir_name;
-    std::string meta_template_name;
-    std::string data_template_name;
-    std::string meta_ext;
-    std::string data_ext;
-    std::vector<pointRecord>* recordVect;
-    std::vector<pointRecord>* playVect;
-} fileData;
-
-// posizione degli elementi nella riga del csv
-enum VPOS {X,Y,Z,RX,RY,RZ,DT,DS,VMAX};
-
 bool openFile(internalState* is, fileData* fd, nlohmann::json meta);
 bool closeFile(internalState* is, fileData* fd);
 
@@ -331,9 +318,7 @@ int main(int argc, char **argv) {
             rviz_data.vtools->loadRemoteControl();
             rviz_data.vtools->deleteAllMarkers();
             rviz_data.vtools->trigger();
-            common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
+            declareStartup(&publishers, &common_par);
-            ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str());
-            publishers[interface_pub.getTopic()]->publish(common_par.startupMsg);
             ros::spinOnce();
         }
         ROS_DEBUG_ONCE("Recorder Node Looping");
@@ -445,6 +430,7 @@ int main(int argc, char **argv) {
         }
         loop_rate.sleep();
     }
+    declareShutdown(&publishers, &common_par);
     return 0;
 }
 ////////////////////////////////////////

src/roboglue_ros/src/roboglue_relay.cpp

@@ -172,9 +172,7 @@ int main(int argc, char **argv) {
     while (ros::ok()) {
         if (startCycle){
             startCycle = false;
-            common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
+            declareStartup(&publishers, &common_par);
-            ROS_DEBUG("StartupMessage: %s", common_par.startupMsg.data.c_str());
-            publishers[interface_pub.getTopic()]->publish(common_par.startupMsg);
             ros::spinOnce();
         }
         ROS_DEBUG_ONCE("Relay Node Looping");
@@ -189,6 +187,6 @@ 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

@@ -17,6 +17,19 @@ bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) {
     nh->getParam(p->param_ns+"loop_rate", p->loopRate);
     nh->getParam(p->param_ns+"msg_buffer", p->msgBufferSize);
     nh->getParam(p->param_ns+"startup_msg", p->startupMsg.data);
+    nh->getParam(p->param_ns+"shutdown_msg", p->shutdownMsg.data);
+}
+
+void declareStartup(publisherMap* publishers, COMMON_parameters* common_par){
+    common_par->startupMsg.data = common_par->startupMsg.data + ros::this_node::getName();
+    ROS_DEBUG("StartupMessage: %s", common_par->startupMsg.data.c_str());
+    publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->startupMsg);
+}
+
+void declareShutdown(publisherMap* publishers, COMMON_parameters* common_par){
+    common_par->shutdownMsg.data = common_par->shutdownMsg.data + ros::this_node::getName();
+    ROS_DEBUG("StartupMessage: %s", common_par->shutdownMsg.data.c_str());
+    publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->shutdownMsg);
 }
 
 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){