Aggiunta funzione di heartbeat sul topic interface

Raggruppati tutti i parametri comuni in una struttura uguale per tutti i
nodi.
Aggiungere altri parametri alla lista dei comuni

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-28T14:03:33. -->
+<!-- Written by QtCreator 4.9.1, 2019-10-29T13:19:39. -->
 <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=392</value>
+     <value type="QString">SHLVL=446</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -47,6 +47,14 @@ typedef struct {
     std::string delta_jog, delta_joint_jog, traj_jog;
 } ROS_topics;
 
+///////// COMMON PARAMETERS //////////////
+typedef struct {
+    ROS_topics ros_topics;
+    int loopRate, msgBufferSize;
+    std::string param_ns;
+    std_msgs::String startupMsg;
+} COMMON_parameters;
+
 ///////// AUX DATA STORAGE ///////////////
 typedef struct {
     std::list<std::pair<std::string,bool>> digital;
@@ -105,6 +113,9 @@ typedef struct {
     mvt::MoveItVisualToolsPtr vtools;
 } rvizData;
 
+///////// LOAD COMMON ROS TOPICS AND PARAMETERS /////////////////////
+bool loadCommonParameters(ros::NodeHandle*, COMMON_parameters*);
+
 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json);
 trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json);
 

src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch

@@ -1,6 +1,6 @@
 <launch>
     <!-- Set logger format -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}][${line}]: ${message}" />
     <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->
 
     <!-- Start ROS official URobotics controller -->
@@ -35,6 +35,7 @@
         <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="loop_rate" type="int" value="100" />
         <param name="msg_buffer" type="int" value="100" />
     </group>
@@ -42,7 +43,6 @@
     <!-- Message Relay to/from MQTT -->
     <node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
-        <param name="startup_msg" type="string" value="STARTUP_RELAY"/>
         <param name="mqtt_host" type="string" value="tcp://localhost:1883" />
         <param name="mqtt_qos" type="int" value="0" />
         <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
@@ -58,14 +58,12 @@
     <!-- Real Time Tracking -->
     <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
         <param name="parameter_ns" type="string" value="roboglue_ros_common" />
-        <param name="startup_msg" type="string" value="STARTUP_FOLLOWER"/>
         <param name="jog_pub_rate" type="int" value="5" />
     </node>
 
     <!-- Tracking data Recorder/Player -->
     <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
-        <param name="startup_msg" type="string" value="STARTUP_RECORDER"/>
         <param name="point_group_mode" type="string" value="dist" />
         <param name="planning_mode" type="string" value="path" />
         <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />

src/roboglue_ros/src/roboglue_follower.cpp

@@ -113,6 +113,16 @@ void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, aux
     ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
 }
 
+void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
+    ROS_WARN("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 timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){
     static geometry_msgs::TwistStamped lasttwPose;
     static trajectory_msgs::JointTrajectory lastjtPose;
@@ -176,77 +186,71 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa
 ////////////////////////////////////////
 int main(int argc, char **argv) {
     /// node variables ///
-    int loopRate, msgBufferSize, jogPubRate;
-    std::string param_ns;
-    std::string startup_msg_tmp;
-    std_msgs::String startup_msg;
-    ROS_topics ros_topics;
+    COMMON_parameters common_par;
     /// internal state struct ////
     internalState is;
     positionData pos_data;
     robotData robot_data;
     auxData aux_data;
     std::vector<std::string> joint_names;
+    int jogPubRate;
 
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_follower");
     ros::NodeHandle nh;
+    publisherMap publishers;
     ///read parameters from server ///
-    nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
-    nh.getParam(param_ns+"startup_msg", startup_msg_tmp);
-    nh.getParam(param_ns+"loop_rate", loopRate);
-    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
-    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
-    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
-    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
-    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
-    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
-    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
-    nh.getParam(param_ns+"OUTdelta_jog", ros_topics.delta_jog);
-    nh.getParam(param_ns+"OUTdelta_joint_jog", ros_topics.delta_joint_jog);
-    nh.getParam(param_ns+"OUTtraj_jog", ros_topics.traj_jog);
-    nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
-    nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
-    nh.getParam(param_ns+"jointHomePosition", robot_data.joint_homes);
+    nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
+    loadCommonParameters(&nh, &common_par);
+    // load robot model information
+    nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam(common_par.param_ns+"jointHomePosition", robot_data.joint_homes);
+    // load jog coord publish rate
     nh.getParam("/roboglue_ros_follower/jog_pub_rate", jogPubRate);
 
+
     /// set spinner rate ///
-    ros::Rate loop_rate(loopRate);
+    ros::Rate loop_rate(common_par.loopRate);
     /// set console log level ///
     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
     ROS_INFO("Follower Node Started");
 
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
+    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                                    boost::bind(coordinateCallback, _1, &publishers, &pos_data));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfaceSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(interfaceCallback, _1, &is));
     /// advertise publish topics ///
-    publisherMap publishers;
-    ros::Publisher jog_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_jog,
-                                                                       static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_joint_jog,
-                                                                             static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(ros_topics.traj_jog,
-                                                                            static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandPub,
-                                                                static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(ros_topics.coordPub,
-                                                              static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.statePub,
-                                                              static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher jog_pub = nh.advertise<geometry_msgs::TwistStamped>(common_par.ros_topics.delta_jog,
+                                                                       static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(common_par.ros_topics.delta_joint_jog,
+                                                                             static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(common_par.ros_topics.traj_jog,
+                                                                            static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordPub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.statePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = 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[coord_pub.getTopic()] = &coord_pub;
     publishers[state_pub.getTopic()] = &state_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
     /// publishers to jog command interfaces
     publishers["jog_pub"] = &jog_pub;
     publishers["jog_joint_pub"] = &jog_joint_pub;
     publishers["trj_pub"] = &trj_pub;
 
-    /// subscribe to incoming topics ///
-    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
-                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
-    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
-                                                                    boost::bind(coordinateCallback, _1, &publishers, &pos_data));
-    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
-                                                               boost::bind(stateCallback, _1, &is, &aux_data));
     /// load the robot model for inverse kinematics and self collision checking ///
     /// requires parameter server and moveIT to be active
     moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
@@ -260,15 +264,17 @@ int main(int argc, char **argv) {
     /// create timed callbacks ///
     ros::Timer timedPublish = nh.createTimer(ros::Duration(1.0/static_cast<double>(jogPubRate)),
                                              boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data));
-    // now we can get inverse kinematics from kinematic state using the joint model
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                         boost::bind(heartBeatCallback, _1, &publishers));
 
     ///////////////////// MAIN LOOP //////////////////////
     bool startCycle = true;
     while (ros::ok() && is.isRunning) {
         if (startCycle){
             startCycle = false;
-            startup_msg.data = startup_msg_tmp.c_str();
-            publishers[command_pub.getTopic()]->publish(startup_msg);
+            common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
+            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");

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -204,15 +204,22 @@ 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());
+}
+
+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);
+};
+
 ////////////////////////////////////////
 //////////////// MAIN //////////////////
 ////////////////////////////////////////
 int main(int argc, char **argv) {
     /// node variables ////
-    int loopRate, msgBufferSize;
-    double nonzero_move;
-    std::string param_ns;
-    ROS_topics ros_topics;
+    COMMON_parameters common_par;
     /// internal node state variables ///
     internalState is;
     auxData aux_data;
@@ -226,11 +233,9 @@ int main(int argc, char **argv) {
     std::vector<pointRecord> playVector;
     file_data.recordVect = &recordVector;
     file_data.playVect = &playVector;
+    double nonzero_move;
 
     /// player variables ///
-
-    std_msgs::String startup_msg;
-    std::string startup_msg_tmp;
     double dsCounter, dtCounter;
     std::vector<pointRecord> tempPlayVector;
     std::vector<geometry_msgs::PoseStamped> tempPlanningVector;
@@ -239,21 +244,19 @@ int main(int argc, char **argv) {
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_recorder");
     ros::NodeHandle nh;
+    publisherMap publishers;
     /// read parameters from server ///
     //load common parameters
-    nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns);
-    nh.getParam(param_ns+"startup_msg", startup_msg_tmp);
-    nh.getParam(param_ns+"loop_rate", loopRate);
-    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
-    nh.getParam(param_ns+"min_nonzero_move", nonzero_move);
-    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
-    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
-    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
-    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
-    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
-    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
+    nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
+    loadCommonParameters(&nh, &common_par);
+    // TODO: inserire anche questi parametri tra quelli comuni
+    // load robot model information
+    nh.getParam(common_par.param_ns+"min_nonzero_move", nonzero_move);
+    nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam(common_par.param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
     // load spin rate
-    nh.getParam("/roboglue_ros_recorder/loop_rate", loopRate);
+    nh.getParam("/roboglue_ros_recorder/loop_rate", common_par.loopRate);
     //load meta template filename
     nh.getParam("/roboglue_ros_recorder/meta_dir", file_data.meta_dir_name);
     nh.getParam("/roboglue_ros_recorder/meta_template", file_data.meta_template_name);
@@ -266,37 +269,42 @@ int main(int argc, char **argv) {
     // load coordinate send operation mode (time/distance)
     nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode);
     nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode);
-    // load robot model information
-    nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
-    nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
-    nh.getParam(param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
+
 
     /// set console log level ///
     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
     /// set spinner rate ///
-    ros::Rate loop_rate(loopRate);
-
+    ros::Rate loop_rate(common_par.loopRate);
     ROS_INFO("Recorder Node Started");
 
-    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
                                                                  boost::bind(commandCallback, _1, &is, &pos_data, &rviz_data, &file_data));
-    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordSub, static_cast<uint32_t>(common_par.msgBufferSize),
                                                                     boost::bind(coordinateCallback, _1, &is, &pos_data));
-    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
                                                                boost::bind(stateCallback, _1, &is, &aux_data));
+    ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfaceSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(interfaceCallback, _1, &is));
     /// advertise publish topics ///
-    publisherMap publishers;
-    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandPub,
-                                                                static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(ros_topics.coordPub,
-                                                              static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.statePub,
-                                                              static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.coordPub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.statePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = 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[coord_pub.getTopic()] = &coord_pub;
     publishers[state_pub.getTopic()] = &state_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
+
+    /// create timed callbacks ///
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                         boost::bind(heartBeatCallback, _1, &publishers));
 
     /// load the robot model for inverse kinematics and self collision checking ///
     /// requires parameter server and moveIT to be active
@@ -319,8 +327,9 @@ int main(int argc, char **argv) {
             rviz_data.vtools->loadRemoteControl();
             rviz_data.vtools->deleteAllMarkers();
             rviz_data.vtools->trigger();
-            startup_msg.data = startup_msg_tmp.c_str();
-            publishers[command_pub.getTopic()]->publish(startup_msg);
+            common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
+            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");

src/roboglue_ros/src/roboglue_relay.cpp

@@ -38,23 +38,22 @@ class mqtt2rosClass: public virtual mqtt::callback {
              ROS_DEBUG("MessageDelivered, t:%d",tok->get_message_id());
         }
         void message_arrived(mqtt::const_message_ptr msg) override{
-            ROS_DEBUG("MqttMessage!");
             std::string topic(msg->get_topic());
             std::string payload(msg->get_payload_str());
             std_msgs::String rosMsg;
             rosMsg.data = payload;
 
             if (!topic.compare(mqtt_data_.MQTTcommandSub)){
-                ROS_DEBUG("Command! -> %s", ros_data_.commandSub.c_str());
+                ROS_DEBUG("MQTT Message: Command! -> %s", ros_data_.commandSub.c_str());
                 pubs_[ros_data_.commandSub]->publish(rosMsg);
             } else if (!topic.compare(mqtt_data_.MQTTcoordSub)){
-                ROS_DEBUG("Coordinate! -> %s", ros_data_.coordSub.c_str());
+                ROS_DEBUG("MQTT Message: Coordinate! -> %s", ros_data_.coordSub.c_str());
                 pubs_[ros_data_.coordSub]->publish(rosMsg);
             } else if (!topic.compare(mqtt_data_.MQTTstateSub)){
-                ROS_DEBUG("State! -> %s", ros_data_.stateSub.c_str());
+                ROS_DEBUG("MQTT Message: State! -> %s", ros_data_.stateSub.c_str());
                 pubs_[ros_data_.stateSub]->publish(rosMsg);
             } else if (!topic.compare(mqtt_data_.MQTTinterfaceSub)) {
-                ROS_DEBUG("Interface! -> %s", ros_data_.interfaceSub.c_str());
+                ROS_DEBUG("MQTT Message: Interface! -> %s", ros_data_.interfaceSub.c_str());
                 pubs_[ros_data_.interfaceSub]->publish(rosMsg);
             } else {
                 ROS_ERROR("Topic NOT Implemented");
@@ -66,49 +65,42 @@ class mqtt2rosClass: public virtual mqtt::callback {
 /////////////// ROS CALLBACKS //////////
 ////////////////////////////////////////
 void commandCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-    ROS_DEBUG("RosMessage: Command!");
+    ROS_DEBUG("ROS Message: Command! -> %s", msg->data.c_str());
     cli_->publish(mqtt::make_message(mqtttopics->MQTTcommandPub, msg->data.c_str()));
 }
 void poseCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-    ROS_DEBUG("RosMessage: Pose!");
+    ROS_DEBUG("ROS Message: Pose! -> %s", msg->data.c_str());
     cli_->publish(mqtt::make_message(mqtttopics->MQTTcoordPub, msg->data.c_str()));
 }
 void stateCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-    ROS_DEBUG("RosMessage: State!");
+    ROS_DEBUG("ROS Message: State! -> %s", msg->data.c_str());
     cli_->publish(mqtt::make_message(mqtttopics->MQTTstatePub, msg->data.c_str()));
 }
 void interfaceCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
-   ROS_DEBUG("RosMessage: Interface!");
+   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){
+    std_msgs::String hbMsg;
+    hbMsg.data = std::string("HB/"+ros::this_node::getName());
+    p->at("/roboglue_com/ros2com/interface")->publish(hbMsg);
+};
+
 ////////////////////////////////////////
 //////////////// MAIN //////////////////
 ////////////////////////////////////////
 int main(int argc, char **argv) {
     /// node variables ///
-    int loopRate, msgBufferSize;
-    std::string param_ns;
+
+    COMMON_parameters common_par;
     MQTT_topics mqtt_topics;
-    ROS_topics ros_topics;
-    std::string startup_msg_tmp;
-    std_msgs::String startup_msg;
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_relay");
     ros::NodeHandle nh;
     ///read parameters from server ///
-    nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
-    nh.getParam(param_ns+"startup_msg", startup_msg_tmp);
-    nh.getParam(param_ns+"loop_rate", loopRate);
-    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
-    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
-    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
-    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
-    nh.getParam(param_ns+"INinterface", ros_topics.interfaceSub);
-    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
-    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
-    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
-    nh.getParam(param_ns+"OUTinterface", ros_topics.interfacePub);
+    nh.getParam("/roboglue_ros_relay/parameter_ns", common_par.param_ns);
+    loadCommonParameters(&nh, &common_par);
     nh.getParam("/roboglue_ros_relay/mqtt_INstate", mqtt_topics.MQTTstateSub);
     nh.getParam("/roboglue_ros_relay/mqtt_INcommands", mqtt_topics.MQTTcommandSub);
     nh.getParam("/roboglue_ros_relay/mqtt_INcoordinates", mqtt_topics.MQTTcoordSub);
@@ -121,37 +113,40 @@ int main(int argc, char **argv) {
     nh.getParam("/roboglue_ros_relay/mqtt_qos", mqtt_topics.mqttQoS);
 
     /// set spinner rate ///
-    ros::Rate loop_rate(loopRate);
+    ros::Rate loop_rate(common_par.loopRate);
     /// 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);
     ROS_INFO("Relay Node Started");
     /// initialize MQTT client ///
     mqtt::async_client client(mqtt_topics.mqttHost,"");
 
     /// advertise publish topics ///
     publisherMap publishers;
-    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandSub,static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher pose_pub = nh.advertise<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.stateSub,static_cast<uint32_t>(msgBufferSize));
-    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(ros_topics.interfaceSub,static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandSub,static_cast<uint32_t>(common_par.msgBufferSize));
+    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));
     /// 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;
     /// create mqtt callback ///
-    mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, ros_topics);
+    mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, common_par.ros_topics);
 
     /// subscribe to incoming topics ///
-    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandPub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandPub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                                  boost::bind(commandCallback, _1, &client, &mqtt_topics));
-    ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(ros_topics.coordPub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.coordPub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                               boost::bind(poseCallback, _1, &client, &mqtt_topics));
-    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.statePub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.statePub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                            boost::bind(stateCallback, _1, &client, &mqtt_topics));
-   ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(ros_topics.interfacePub,static_cast<uint32_t>(msgBufferSize),
+    ros::Subscriber interface_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.interfacePub,static_cast<uint32_t>(common_par.msgBufferSize),
                                                               boost::bind(interfaceCallback, _1, &client, &mqtt_topics));
 
+    /// create timed callbacks ///
+    ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
+                                         boost::bind(heartBeatCallback, _1, &publishers));
      /// CONNECT MQTT CLIENT ///
     ROS_INFO("Connecting to MQTT...");
     try {
@@ -171,8 +166,9 @@ int main(int argc, char **argv) {
     while (ros::ok()) {
         if (startCycle){
             startCycle = false;
-            startup_msg.data = startup_msg_tmp.c_str();
-            publishers[command_pub.getTopic()]->publish(startup_msg);
+            common_par.startupMsg.data = common_par.startupMsg.data + ros::this_node::getName();
+            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");

src/roboglue_ros/src/roboglue_utils.cpp

@@ -2,6 +2,23 @@
 
 /// Utilities funtion used in roboglue ros nodes ///
 
+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);
+    nh->getParam(p->param_ns+"INcoordinates", p->ros_topics.coordSub);
+    nh->getParam(p->param_ns+"INinterface", p->ros_topics.interfaceSub);
+    nh->getParam(p->param_ns+"OUTstate", p->ros_topics.statePub);
+    nh->getParam(p->param_ns+"OUTcommands", p->ros_topics.commandPub);
+    nh->getParam(p->param_ns+"OUTcoordinates", p->ros_topics.coordPub);
+    nh->getParam(p->param_ns+"OUTinterface", p->ros_topics.interfacePub);
+    nh->getParam(p->param_ns+"OUTdelta_jog", p->ros_topics.delta_jog);
+    nh->getParam(p->param_ns+"OUTdelta_joint_jog", p->ros_topics.delta_joint_jog);
+    nh->getParam(p->param_ns+"OUTtraj_jog", p->ros_topics.traj_jog);
+    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);
+}
+
 geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){
     static uint32_t seqId = 0;
     geometry_msgs::TwistStamped tw;