From 8778f69eb9aaa3eef341f884ef0b3fc54ccd037b Mon Sep 17 00:00:00 2001 From: Emanuele Date: Tue, 29 Oct 2019 11:02:03 +0100 Subject: [PATCH] Aggiunti topi MQTT e ROS aggiunti topic interface per la comunicazione di stato tra i moduli e l'interfacia Qt (e viceversa) --- .../include/roboglue_ros/roboglue_utils.h | 8 ++++---- src/roboglue_ros/src/roboglue_relay.cpp | 18 +++++++++++++++++- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 3cb4a18..67ed80e 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/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 MQTTcommandSub, MQTTcoordSub, MQTTstateSub; + std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub, MQTTinterfacePub; + std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub, MQTTinterfaceSub; std::string mqttHost; int mqttQoS; } MQTT_topics; typedef struct { - std::string commandPub, coordPub, statePub; - std::string commandSub, coordSub, stateSub; + std::string commandPub, coordPub, statePub, interfacePub; + std::string commandSub, coordSub, stateSub, interfaceSub; std::string delta_jog, delta_joint_jog, traj_jog; } ROS_topics; diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 775c687..847a5b1 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/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(ros_topics.commandSub,static_cast(msgBufferSize)); ros::Publisher pose_pub = nh.advertise(ros_topics.coordSub, static_cast(msgBufferSize)); ros::Publisher state_pub = nh.advertise(ros_topics.stateSub,static_cast(msgBufferSize)); + ros::Publisher interface_pub = nh.advertise(ros_topics.interfaceSub,static_cast(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(ros_topics.coordPub,static_cast(msgBufferSize), boost::bind(poseCallback, _1, &client, &mqtt_topics)); ros::Subscriber state_sub = nh.subscribe(ros_topics.statePub,static_cast(msgBufferSize), - boost::bind(stateCallback, _1, &client, &mqtt_topics)); + boost::bind(stateCallback, _1, &client, &mqtt_topics)); + ros::Subscriber interface_sub = nh.subscribe(ros_topics.interfacePub,static_cast(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());