diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index d0c0972..5434a21 100644 --- a/roboglue_ros_ws.workspace.user +++ b/roboglue_ros_ws.workspace.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -124,7 +124,7 @@ ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share ROS_ROOT=/opt/ros/kinetic/share/ros ROS_VERSION=1 - SHLVL=608 + SHLVL=614 TERM=xterm _=/usr/bin/env diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch index 5147755..fb8c8f1 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch @@ -35,8 +35,9 @@ - - + + + diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 252a483..8646523 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/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()); diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 0bf3882..00509f9 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/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()); diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 27ab7ab..d61eee1 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/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()); diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index 3ef49c8..fd4f7ac 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/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); }