Prove per il messaggio di startup

non carica il messaggio dal server dei parametri, no si può pubblicare
un tipo std::string in un topic ROS

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -40,7 +40,7 @@
     <!-- 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="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" />
@@ -54,14 +54,14 @@
     <!-- 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="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="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/launch/roboglue_ros_urdriver_norviz.launch

@@ -40,6 +40,7 @@
     <!-- 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" />
@@ -53,12 +54,14 @@
     <!-- 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

@@ -178,6 +178,7 @@ 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;
     /// internal state struct ////
@@ -192,7 +193,7 @@ int main(int argc, char **argv) {
     ros::NodeHandle nh;
     ///read parameters from server ///
     nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
-    nh.getParam(param_ns+"startup_msg", startup_msg.data);
+    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);
@@ -266,7 +267,9 @@ int main(int argc, char **argv) {
     while (ros::ok() && is.isRunning) {
         if (startCycle){
             startCycle = false;
+            startup_msg.data = startup_msg_tmp.c_str();
             publishers[command_pub.getTopic()]->publish(startup_msg);
+            ros::spinOnce();
         }
         ROS_DEBUG_ONCE("Follower Node Looping");
         ros::spinOnce();

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -230,6 +230,7 @@ int main(int argc, char **argv) {
     /// 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;
@@ -241,7 +242,7 @@ int main(int argc, char **argv) {
     /// read parameters from server ///
     //load common parameters
     nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns);
-    nh.getParam(param_ns+"startup_msg", startup_msg.data);
+    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);
@@ -318,7 +319,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);
+            ros::spinOnce();
         }
         ROS_DEBUG_ONCE("Recorder Node Looping");
         ros::spinOnce();

src/roboglue_ros/src/roboglue_relay.cpp

@@ -84,13 +84,14 @@ int main(int argc, char **argv) {
     std::string param_ns;
     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.data);
+    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);
@@ -154,7 +155,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);
+            ros::spinOnce();
         }
         ROS_DEBUG_ONCE("Relay Node Looping");
         ros::spinOnce();