publish all "other" MQTT messages as generic ros messages

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