Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Aggiunti topi MQTT e ROS

Browse Source 
aggiunti topic interface per la comunicazione di stato tra i moduli e
l'interfacia Qt (e viceversa)
This commit is contained in:
Emanuele
2019-10-29 11:02:03 +01:00
parent db4a3fab14
commit 8778f69eb9
2 changed files with 21 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
View File

@@ -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;

18
src/roboglue_ros/src/roboglue_relay.cpp
View File

@@ -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());