From 1bd83a4ab2399f8bee7bc42d7decfa96affc0962 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Tue, 14 Jul 2020 23:28:27 +0200 Subject: [PATCH] remove data config mqtt interface, add it to config file --- config/config.yaml | 4 ++-- proto/config.proto | 5 ----- src/MqttRosConnectionTestNode.cpp | 18 +----------------- src/MqttToRosNode.cpp | 18 ------------------ 4 files changed, 3 insertions(+), 42 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index e5c9b01..a7dc5c3 100644 --- a/config/config.yaml +++ b/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 diff --git a/proto/config.proto b/proto/config.proto index 74f231c..38cb5d0 100644 --- a/proto/config.proto +++ b/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 diff --git a/src/MqttRosConnectionTestNode.cpp b/src/MqttRosConnectionTestNode.cpp index d4ca978..2e3142b 100644 --- a/src/MqttRosConnectionTestNode.cpp +++ b/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"); diff --git a/src/MqttToRosNode.cpp b/src/MqttToRosNode.cpp index b728ed1..8854df7 100644 --- a/src/MqttToRosNode.cpp +++ b/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; -- GitLab