Aggiunti topi MQTT e ROS

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/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);
 
@@ -135,7 +148,9 @@ int main(int argc, char **argv) {
     ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(ros_topics.coordPub,static_cast<uint32_t>(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),
-                                                               boost::bind(stateCallback, _1, &client, &mqtt_topics));
+                                                           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());