From 6c25a4d33e684c9de6405bd63756aca8e18c63ea Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Tue, 21 Jul 2020 01:16:51 +0200 Subject: [PATCH] publish all "other" MQTT messages as generic ros messages --- src/MqttToRosNode.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp index 11de299..da5e257 100644 --- a/src/MqttToRosNode.cpp +++ b/src/MqttToRosNode.cpp @@ -6,6 +6,7 @@ #include "panda_mqtt_connector/Trajectory.h" #include "panda_mqtt_connector/Waypoint.h" +#include "panda_mqtt_connector/MqttMessage.h" #include "std_msgs/Float64.h" #include "config.pb.h" @@ -20,14 +21,14 @@ const std::string TRAJECTORY_CONFIG{"trajectory"}; MqttUtil *mqttUtil = nullptr; void handleRobotConfig(const config::RobotConfig &robotConfig, const ros::Publisher &velocityPublisher) { - ROS_INFO_STREAM("Retrieved new target-speed: " << robotConfig.speed()); + ROS_INFO_STREAM("Received new target-speed: " << robotConfig.speed()); std_msgs::Float64 velocity; velocity.data = robotConfig.speed(); velocityPublisher.publish(velocity); } void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Publisher &trajectoryPublisher) { - + ROS_INFO_STREAM("Received new trajectory with " << protoTrajectory.pose().size() << " points."); panda_mqtt_connector::Trajectory trajectory; for (const auto &protoPose : protoTrajectory.pose()) { @@ -64,11 +65,10 @@ void handleNewTrajectory(const plan::Trajectory &protoTrajectory, const ros::Pub } -void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher, const ros::Publisher &velocityPublisher) { +void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &trajectoryPublisher, const ros::Publisher &velocityPublisher, const ros::Publisher &mqttMessagePublisher) { if (mqttUtil->ensureConnection()) { mqtt::const_message_ptr msg; if (mqttUtil->getClient().try_consume_message_for(&msg, std::chrono::milliseconds(500))) { - ROS_INFO_STREAM("NEW MESSAGE ON TOPIC " << msg.get()->get_topic()); if (msg->get_topic() == ROBOT_CONFIG) { const std::string rc_payload = msg->get_payload_str(); config::RobotConfig robotConfig; @@ -79,6 +79,14 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &traject plan::Trajectory trajectoryConfig; trajectoryConfig.ParseFromString(tc_payload); handleNewTrajectory(trajectoryConfig, trajectoryPublisher); + } else { + ROS_INFO_STREAM("Retrieved new MQTT message on topic " << msg->get_topic()); + panda_mqtt_connector::MqttMessage mqttMessage; + mqttMessage.topic = msg->get_topic(); + for (auto b : msg->get_payload()) { + mqttMessage.content.push_back(b); + } + mqttMessagePublisher.publish(mqttMessage); } } } else { @@ -92,6 +100,7 @@ int main(int argc, char *argv[]) { ros::Publisher trajectoryPublisher = n.advertise<panda_mqtt_connector::Trajectory>("trajectory_update", 1000); ros::Publisher maxVelocityPublisher = n.advertise<std_msgs::Float64>("max_velocity", 1000); + ros::Publisher mqttMessagePublisher = n.advertise<panda_mqtt_connector::MqttMessage>("mqtt_message", 1000); std::string server; if (!n.getParam("server", server)) { @@ -101,8 +110,6 @@ int main(int argc, char *argv[]) { mqttUtil = new MqttUtil(CLIENT_ID, server); - std::string robotConfig = "robotconfig"; - std::map<std::string, std::string> topics; if (!n.getParam("topics", topics)) { ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/topics from param server"); @@ -119,7 +126,7 @@ int main(int argc, char *argv[]) { } while (ros::ok()) { - receiveMqttMessages(n, trajectoryPublisher, maxVelocityPublisher); + receiveMqttMessages(n, trajectoryPublisher, maxVelocityPublisher, mqttMessagePublisher); ros::spinOnce(); } -- GitLab