Modificate le funzioni startup e heartbeat

aggiunti timestamp,
da aggiungere un modo per far emettere un messaggio di shutdown prima
che il nodo venga killato.

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-11-04T11:07:27. -->
+<!-- Written by QtCreator 4.9.1, 2019-11-04T11:24:53. -->
 <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=608</value>
+     <value type="QString">SHLVL=614</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>

src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch

@@ -35,8 +35,9 @@
         <param name="joint_jump_tresh" type="double" value="45.0" />
         <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
         <!-- ROS node loop parameters -->
-        <param name="startup_msg" type="string" value="STARTUP/"/>
+        <param name="startup_msg" type="string" value="STARTUP"/>
-        <param name="shutdown_msg" type="string" value="SHUTDOWN/"/>
+        <param name="shutdown_msg" type="string" value="SHUTDOWN"/>
+        <param name="heartbeat_msg" type="string" value="HB"/>
         <param name="loop_rate" type="int" value="100" />
         <param name="msg_buffer" type="int" value="100" />
     </group>

src/roboglue_ros/src/roboglue_follower.cpp

@@ -124,7 +124,9 @@ void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is)
 void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
     try {
         std_msgs::String hbMsg;
-        hbMsg.data = std::string("HB/"+ros::this_node::getName()+"//"+std::to_string(t.current_real.toSec()));
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
         p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
     } catch (std::exception &e) {
         ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -198,7 +198,9 @@ void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is)
 void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
     try {
         std_msgs::String hbMsg;
-        hbMsg.data = std::string("HB/"+ros::this_node::getName()+"//"+std::to_string(t.current_real.toSec()));
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
         p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
     } catch (std::exception &e) {
         ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());

src/roboglue_ros/src/roboglue_relay.cpp

@@ -83,7 +83,10 @@ void interfaceCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client*
 void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
     std_msgs::String hbMsg;
     try {
-        hbMsg.data = std::string("HB/"+ros::this_node::getName()+"//"+std::to_string(t.current_real.toSec()));
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
         p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
     } catch (std::exception &e) {
         ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());

src/roboglue_ros/src/roboglue_utils.cpp

@@ -24,13 +24,17 @@ bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) {
 }
 
 void declareStartup(publisherMap* publishers, COMMON_parameters* common_par){
-    common_par->startupMsg.data = common_par->startupMsg.data + ros::this_node::getName();
+    common_par->startupMsg.data = common_par->startupMsg.data + ":" +
+                                    boost::replace_all_copy(ros::this_node::getName(), "/", "") +
+                                    ":" + std::to_string(ros::Time::now().toSec());
     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();
+    common_par->shutdownMsg.data = common_par->shutdownMsg.data + ":" +
+                                    boost::replace_all_copy(ros::this_node::getName(), "/", "") +
+                                    ":" + std::to_string(ros::Time::now().toSec());
     ROS_DEBUG("StartupMessage: %s", common_par->shutdownMsg.data.c_str());
     publishers->find(common_par->ros_topics.interfacePub)->second->publish(common_par->shutdownMsg);
 }