remove data config mqtt interface, add it to config file

config/config.yaml

@@ -2,10 +2,10 @@ panda_mqtt_connector:
   server: "tcp://localhost:1883"
   robot_speed_factor: .7
   robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
-  default_trajectory: "<none>" # "square" or "circle", everything else is ignored
+  default_trajectory: "<none>"      # "square" or "circle", everything else is ignored
+  data_publish_rate: 50.0           # publish every x'th robot state message
   topics:
     robotConfig: "robotconfig"
-    dataConfig: "dataconfig"
     trajectory: "trajectory"
     nextStep: "ros2rag/nextStep"
   zone_size: 0.5

proto/config.proto

@@ -4,9 +4,4 @@ package config;
 
 message RobotConfig {
   double speed = 1;
-}
-
-message DataConfig {
-  repeated string links = 1;
-  double publishRate = 2;
 }
\ No newline at end of file

src/MqttRosConnectionTestNode.cpp

@@ -71,28 +71,13 @@ void testSpeedFactorChange(const ros::NodeHandle &n) {
   mqttUtil->getClient().publish(pubmsg);
 }
 
-void testDataConfig(const ros::NodeHandle &n) {
-  ROS_INFO_STREAM("DATA CONFIG TEST: publishing all data at a rate of 1000");
-
-  config::DataConfig dc;
-  dc.set_publishrate(1000);
-
-  std::string mqtt_msg;
-  dc.SerializeToString(&mqtt_msg);
-
-  auto pubmsg = mqtt::make_message("dataconfig", mqtt_msg);
-  mqttUtil->getClient().publish(pubmsg);
-}
-
 void receiveMqttMessages(const ros::NodeHandle &n) {
 
   if (mqttUtil->ensureConnection()) {
     auto *msg = new mqtt::const_message_ptr;
     if (mqttUtil->getClient().try_consume_message_for(msg, std::chrono::milliseconds(500))) {
       ROS_INFO_STREAM("Received message on topic " << msg->get()->get_topic());
-      if (msg->get()->get_topic() == "test_config") {
-        testDataConfig(n);
-      } else if (msg->get()->get_topic() == "test_speed_change") {
+      if (msg->get()->get_topic() == "test_speed_change") {
         testSpeedFactorChange(n);
       } else if (msg->get()->get_topic() == "test_trajectory_update") {
         testTrajectoryUpdate(n);
@@ -118,7 +103,6 @@ int main(int argc, char **argv) {
 
   mqttUtil = new MqttUtil(CLIENT_ID, server);
 
-  mqttUtil->addTopic("test_config");
   mqttUtil->addTopic("test_speed_change");
   mqttUtil->addTopic("test_trajectory_update");
 

src/MqttToRosNode.cpp

@@ -14,7 +14,6 @@
 const std::string CLIENT_ID{"mqtt_to_ros"};
 
 const std::string ROBOT_CONFIG{"robotconfig"};
-const std::string DATA_CONFIG{"dataconfig"};
 const std::string TRAJECTORY_CONFIG{"trajectory"};
 
 MqttUtil *mqttUtil = nullptr;
@@ -58,18 +57,6 @@ void handleNewTrajectory(const plan::Trajectory &trajectory, const ros::Publishe
 
 }
 
-void handleDataConfig(const config::DataConfig &dataConfig, const ros::NodeHandle &n) {
-
-  std::string links;
-  for (const auto &piece : dataConfig.links()) links = links + " " + "links";
-
-  ROS_INFO_STREAM("Retrieved new data-config. links = " << links << "; rate = " << dataConfig.publishrate());
-
-  n.setParam("data_publish_rate", dataConfig.publishrate());
-
-  // TODO add links to a set
-}
-
 void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePublisher) {
   if (mqttUtil->ensureConnection()) {
     mqtt::const_message_ptr msg;
@@ -80,11 +67,6 @@ void receiveMqttMessages(const ros::NodeHandle &n, const ros::Publisher &posePub
         config::RobotConfig robotConfig;
         robotConfig.ParseFromString(rc_payload);
         handleRobotConfig(robotConfig, n);
-      } else if (msg->get_topic() == DATA_CONFIG) {
-        const std::string dc_payload = msg->get_payload_str();
-        config::DataConfig dataConfig;
-        dataConfig.ParseFromString(dc_payload);
-        handleDataConfig(dataConfig, n);
       } else if (msg->get_topic() == TRAJECTORY_CONFIG) {
         const std::string tc_payload = msg->get_payload_str();
         plan::Trajectory trajectoryConfig;