Aggiornalo launch con i nuovi topic

aggiunti topic interface per la comunicazione di stato tra i moduli e
l'interfacia Qt (e viceversa)

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -35,15 +35,15 @@ namespace mvt=moveit_visual_tools;
 
 ///////// MQTT & ROS TOPIC NAMES ///////////////
 typedef struct {
-    std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub;
+    std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub, MQTTinterfacePub;
-    std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub;
+    std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub, MQTTinterfaceSub;
     std::string mqttHost;
     int mqttQoS;
 } MQTT_topics;
 
 typedef struct {
-    std::string commandPub, coordPub, statePub;
+    std::string commandPub, coordPub, statePub, interfacePub;
-    std::string commandSub, coordSub, stateSub;
+    std::string commandSub, coordSub, stateSub, interfaceSub;
     std::string delta_jog, delta_joint_jog, traj_jog;
 } ROS_topics;
 

src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch

@@ -19,9 +19,11 @@
         <param name="INstate"   type="string" value="/roboglue_com/com2ros/state" />
         <param name="INcommands" type="string" value="/roboglue_com/com2ros/commands" />
         <param name="INcoordinates" type="string" value="/roboglue_com/com2ros/coordinates" />
+        <param name="INinterface" type="string" value="/roboglue_com/com2ros/interface" />
         <param name="OUTstate" type="string" value="/roboglue_com/ros2com/state" />
         <param name="OUTcommands" type="string" value="/roboglue_com/ros2com/commands" />
         <param name="OUTcoordinates" type="string" value="/roboglue_com/ros2com/coordinates" />
+        <param name="OUTinterface" type="string" value="/roboglue_com/ros2com/interface" />
         <!-- ROS movement topics (For UR official driver use scalable position controller) -->
         <param name="OUTdelta_jog" type="string" value="/jog_arm_server/delta_jog_cmds" />
         <param name="OUTdelta_joint_jog" type="string" value="/jog_arm_server/delta_joint_jog_cmds" />
@@ -40,28 +42,30 @@
     <!-- 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" />
         <param name="mqtt_INcommands" type="string" value="roboglue_com/com2ros/commands" />
         <param name="mqtt_INcoordinates" type="string" value="roboglue_com/com2ros/coordinates" />
+        <param name="mqtt_INinterface" type="string" value="roboglue_com/com2ros/interface" />
         <param name="mqtt_OUTstate" type="string" value="roboglue_com/ros2com/state" />
         <param name="mqtt_OUTcommands" type="string" value="roboglue_com/ros2com/commands" />
         <param name="mqtt_OUTcoordinates" type="string" value="roboglue_com/ros2com/coordinates" />
+        <param name="mqtt_OUTinterface" type="string" value="roboglue_com/ros2com/interface" />
     </node>
 
     <!-- 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/src/roboglue_relay.cpp

@@ -53,6 +53,9 @@ class mqtt2rosClass: public virtual mqtt::callback {
             } else if (!topic.compare(mqtt_data_.MQTTstateSub)){
                 ROS_DEBUG("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());
+                pubs_[ros_data_.interfaceSub]->publish(rosMsg);
             } else {
                 ROS_ERROR("Topic NOT Implemented");
             }
@@ -74,6 +77,10 @@ void stateCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli
     ROS_DEBUG("RosMessage: State!");
     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!");
+   cli_->publish(mqtt::make_message(mqtttopics->MQTTinterfacePub, msg->data.c_str()));
+}
 
 ////////////////////////////////////////
 //////////////// MAIN //////////////////
@@ -97,15 +104,19 @@ int main(int argc, char **argv) {
     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/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);
+    nh.getParam("/roboglue_ros_relay/mqtt_INinterface", mqtt_topics.MQTTinterfaceSub);
     nh.getParam("/roboglue_ros_relay/mqtt_OUTstate", mqtt_topics.MQTTstatePub);
     nh.getParam("/roboglue_ros_relay/mqtt_OUTcommands", mqtt_topics.MQTTcommandPub);
     nh.getParam("/roboglue_ros_relay/mqtt_OUTcoordinates", mqtt_topics.MQTTcoordPub);
+    nh.getParam("/roboglue_ros_relay/mqtt_OUTinterface", mqtt_topics.MQTTinterfacePub);
     nh.getParam("/roboglue_ros_relay/mqtt_host", mqtt_topics.mqttHost);
     nh.getParam("/roboglue_ros_relay/mqtt_qos", mqtt_topics.mqttQoS);
 
@@ -122,10 +133,12 @@ int main(int argc, char **argv) {
     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));
     /// 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);
 
@@ -136,6 +149,8 @@ int main(int argc, char **argv) {
                                                               boost::bind(poseCallback, _1, &client, &mqtt_topics));
     ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.statePub,static_cast<uint32_t>(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),
+                                                              boost::bind(interfaceCallback, _1, &client, &mqtt_topics));
 
      /// CONNECT MQTT CLIENT ///
     ROS_INFO("Connecting to MQTT...");
@@ -145,6 +160,7 @@ int main(int argc, char **argv) {
         client.subscribe(mqtt_topics.MQTTcommandSub,mqtt_topics.mqttQoS);
         client.subscribe(mqtt_topics.MQTTcoordSub,mqtt_topics.mqttQoS);
         client.subscribe(mqtt_topics.MQTTstateSub,mqtt_topics.mqttQoS);
+        client.subscribe(mqtt_topics.MQTTinterfaceSub,mqtt_topics.mqttQoS);
         client.start_consuming();
     } catch (mqtt::exception &e) {
         ROS_ERROR("Failed to Connect to MQTT: [%s]", e.what());