Aggiornati i messaggi di heartbeat da mandare a gui con timestamp

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-29T13:19:39. -->
+<!-- Written by QtCreator 4.9.1, 2019-10-30T12:20:28. -->
 <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=446</value>
+     <value type="QString">SHLVL=528</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>
@@ -559,8 +559,8 @@
       <value type="bool" key="ROSProjectManager.ROSGenericStep.DebugContinueOnAttach">true</value>
       <value type="QString" key="ROSProjectManager.ROSGenericStep.Package">roboglue_ros</value>
       <value type="QString" key="ROSProjectManager.ROSGenericStep.PackagePath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros</value>
-      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_recorder</value>
-      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_relay</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_relay</value>
      </valuemap>
      <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.1">
       <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>

src/roboglue_ros/src/roboglue_follower.cpp

@@ -114,14 +114,18 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, aux
 }
 
 void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
-    ROS_WARN("Received Interface Message; [%s]", msg->data.c_str());
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
 }
 
-void heartBeatCallback(const ros::TimerEvent&, publisherMap* p){
-    std_msgs::String hbMsg;
-    hbMsg.data = std::string("HB/"+ros::this_node::getName());
-    p->at("/roboglue_com/ros2com/interface")->publish(hbMsg);
-};
+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()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}
 
 void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){
     static geometry_msgs::TwistStamped lasttwPose;

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -205,14 +205,18 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxD
 }
 
 void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
-    ROS_WARN("Received Interface Message; [%s]", msg->data.c_str());
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
 }
 
-void heartBeatCallback(const ros::TimerEvent&, publisherMap* p){
-    std_msgs::String hbMsg;
-    hbMsg.data = std::string("HB/"+ros::this_node::getName());
-    p->at("/roboglue_com/ros2com/interface")->publish(hbMsg);
-};
+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()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}
 
 ////////////////////////////////////////
 //////////////// MAIN //////////////////

src/roboglue_ros/src/roboglue_relay.cpp

@@ -80,12 +80,15 @@ void interfaceCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client*
    ROS_DEBUG("ROS Message: Interface! -> %s", msg->data.c_str());
    cli_->publish(mqtt::make_message(mqtttopics->MQTTinterfacePub, msg->data.c_str()));
 }
-
-void heartBeatCallback(const ros::TimerEvent&, publisherMap* p){
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
     std_msgs::String hbMsg;
-    hbMsg.data = std::string("HB/"+ros::this_node::getName());
-    p->at("/roboglue_com/ros2com/interface")->publish(hbMsg);
-};
+    try {
+        hbMsg.data = std::string("HB/"+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());
+    }
+}
 
 ////////////////////////////////////////
 //////////////// MAIN //////////////////
@@ -115,7 +118,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::Debug);
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
     ROS_INFO("Relay Node Started");
     /// initialize MQTT client ///
     mqtt::async_client client(mqtt_topics.mqttHost,"");
@@ -126,11 +129,14 @@ int main(int argc, char **argv) {
     ros::Publisher pose_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize));
     ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.stateSub,static_cast<uint32_t>(common_par.msgBufferSize));
     ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfaceSub,static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub2 = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,static_cast<uint32_t>(common_par.msgBufferSize));
+
     /// build a list of publisher to pass to mqtt callback ///
     publishers[command_pub.getTopic()] = &command_pub;
     publishers[pose_pub.getTopic()] = &pose_pub;
     publishers[state_pub.getTopic()] = &state_pub;
     publishers[interface_pub.getTopic()] = &interface_pub;
+    publishers[interface_pub2.getTopic()] = &interface_pub2;
     /// create mqtt callback ///
     mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, common_par.ros_topics);