diff --git a/config/config.yaml b/config/config.yaml
index e891d24f37a989336158d6c3bc1c3af0b90a4cc2..e4952837c71df94621d5fd6d488ab5fdde4de411 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -1,21 +1,23 @@
panda_mqtt_connector:
server: "tcp://localhost:1883"
topics:
- - robotConfig: "robotconfig"
- - dataConfig: "dataconfig"
+ - robotConfig: "robotconfig"
+ - dataConfig: "dataconfig"
zone_size: 0.5
zones:
- "1 1"
- "-1 -1 1"
joints:
- - panda/Joint0: "panda::panda_link0"
- - panda/Joint1: "panda::panda_link1"
- - panda/Joint2: "panda::panda_link2"
- - panda/Joint3: "panda::panda_link3"
- - panda/Joint4: "panda::panda_link4"
- - panda/Joint5: "panda::panda_link5"
- - panda/Joint6: "panda::panda_link6"
- - panda/LeftFinger: "panda::panda_leftfinger"
- - panda/RightFinger: "panda::panda_rightfinger"
+ panda:
+ Joint0: "panda::panda_link0"
+ Joint1: "panda::panda_link1"
+ Joint2: "panda::panda_link2"
+ Joint3: "panda::panda_link3"
+ Joint4: "panda::panda_link4"
+ Joint5: "panda::panda_link5"
+ Joint6: "panda::panda_link6"
+ LeftFinger: "panda::panda_leftfinger"
+ RightFinger: "panda::panda_rightfinger"
end_effectors:
- - panda/EndEffector: "panda::panda_link7"
\ No newline at end of file
+ panda:
+ EndEffector: "panda::panda_link7"
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
index 1875ebb42770a28f90c8869cbf434e77404aace7..bf3b35d3367da1cb8ef1e592622cf1af6983fc09 100644
--- a/src/RobotStateProvider.cpp
+++ b/src/RobotStateProvider.cpp
@@ -1,6 +1,10 @@
-//
-// Created by sebastian on 31.03.20.
-//
+/** @file RobotStateProvider.cpp
+ *
+ * @author Sebastian Ebert
+ * @author Johannes Mey
+ * @date 31.03.20
+*/
+
#include "ros/ros.h"
#include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h>
@@ -9,211 +13,246 @@
#include "util/MqttUtil.h"
-bool export_to_log = false;
+namespace robot_state_provider {
+ bool export_to_log = false;
-/*
- * mqtt-topics for all links
- * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
+/**
+ * A map from the ROS element name to an MQTT topic
*/
-std::set <std::string> topics = {"ground_plane::link", "panda::panda_link0", "panda::panda_link1",
- "panda::panda_link2", "panda::panda_link3", "panda::panda_link4",
- "panda::panda_link5", "panda::panda_link6", "panda::panda_link7",
- "panda::panda_leftfinger", "panda::panda_rightfinger"};
+ std::map<std::string, std::string> topics{};
-bool isInitialized = false;
-const int QOS = 1;
-int current_redirect = 0;
+ constexpr int QOS = 0;
-const std::string CLIENT_ID{"robot_state_provider_mqtt"};
+ const std::string CLIENT_ID = "robot_state_provider_mqtt";
-MqttUtil *mqttUtil = nullptr;
+ MqttUtil *mqttUtil = nullptr;
/*
* logs to its own file in /.ros/logs (configured in launch-file)
*/
-void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
+ void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
- bool export_position = false;
- bool export_orientation = false;
- bool export_twist_linear = false;
- bool export_twist_angular = false;
+ bool export_position = false;
+ bool export_orientation = false;
+ bool export_twist_linear = false;
+ bool export_twist_angular = false;
- ros::NodeHandle n;
+ ros::NodeHandle n;
- n.getParam("data_enable_position", export_position);
- n.getParam("data_enable_orientation", export_orientation);
- n.getParam("data_enable_twist_linear", export_twist_linear);
- n.getParam("data_enable_twist_angular", export_twist_angular);
+ n.getParam("data_enable_position", export_position);
+ n.getParam("data_enable_orientation", export_orientation);
+ n.getParam("data_enable_twist_linear", export_twist_linear);
+ n.getParam("data_enable_twist_angular", export_twist_angular);
- ROS_INFO_STREAM("<<< LINK NAMES >>>");
+ ROS_INFO_STREAM("<<< LINK NAMES >>>");
- for (int i = 0; i < msg.name.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
- }
+ for (int i = 0; i < msg.name.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
+ }
- if (export_position) {
+ if (export_position) {
- ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
+ ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
- for (int i = 0; i < msg.pose.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
+ for (int i = 0; i < msg.pose.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
+ }
}
- }
- if (export_orientation) {
+ if (export_orientation) {
- ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
+ ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
- for (int i = 0; i < msg.pose.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
+ for (int i = 0; i < msg.pose.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
+ }
}
- }
- if (export_twist_linear) {
- ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
- for (int i = 0; i < msg.twist.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
+ if (export_twist_linear) {
+ ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
+ for (int i = 0; i < msg.twist.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
+ }
}
- }
- if (export_twist_angular) {
- ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
- for (int i = 0; i < msg.twist.size(); i++) {
- ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
- ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
- ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
+ if (export_twist_angular) {
+ ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
+ for (int i = 0; i < msg.twist.size(); i++) {
+ ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
+ ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
+ ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
+ }
}
}
-}
-std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
+ std::vector<panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
- bool export_position = false;
- bool export_orientation = false;
- bool export_twist_linear = false;
- bool export_twist_angular = false;
+ bool export_position = false;
+ bool export_orientation = false;
+ bool export_twist_linear = false;
+ bool export_twist_angular = false;
- ros::NodeHandle n("panda_mqtt_connector");
+ ros::NodeHandle n("panda_mqtt_connector");
- if (!n.getParam("data_enable_position", export_position)) {
- export_position = false;
- }
- if (!n.getParam("data_enable_orientation", export_orientation)) {
- export_orientation = false;
- }
- if (!n.getParam("data_enable_twist_linear", export_twist_linear)) {
- export_twist_linear = false;
- }
- if (!n.getParam("data_enable_twist_angular", export_twist_angular)) {
- export_twist_angular = false;
- }
+ if (!n.getParam("data_enable_position", export_position)) {
+ export_position = false;
+ }
+ if (!n.getParam("data_enable_orientation", export_orientation)) {
+ export_orientation = false;
+ }
+ if (!n.getParam("data_enable_twist_linear", export_twist_linear)) {
+ export_twist_linear = false;
+ }
+ if (!n.getParam("data_enable_twist_angular", export_twist_angular)) {
+ export_twist_angular = false;
+ }
- std::vector <panda::PandaLinkState> messages{};
- std::vector <std::string> names{msg.name};
+ std::vector<panda::PandaLinkState> messages{};
+ std::vector<std::string> names{msg.name};
- for (int link_number = 0; link_number < names.size(); ++link_number) {
- std::string name = names[link_number];
- if (topics.find(name) != topics.end()) {
+ for (int link_number = 0; link_number < names.size(); ++link_number) {
+ std::string name = names[link_number];
+ if (topics.find(name) != topics.end()) {
- panda::PandaLinkState pls_msg;
+ panda::PandaLinkState pls_msg;
- pls_msg.set_name(msg.name.at(link_number));
+ // set the name to the MQTT topic provided by the topics map
+ pls_msg.set_name(topics[name]);
- if (export_position) {
- panda::PandaLinkState_Position *pos = new panda::PandaLinkState_Position();
- pos->set_positionx(msg.pose.at(link_number).position.x);
- pos->set_positiony(msg.pose.at(link_number).position.y);
- pos->set_positionz(msg.pose.at(link_number).position.z);
- pls_msg.set_allocated_pos(pos);
- }
+ if (export_position) {
+ auto *pos = new panda::PandaLinkState_Position();
+ pos->set_positionx(msg.pose.at(link_number).position.x);
+ pos->set_positiony(msg.pose.at(link_number).position.y);
+ pos->set_positionz(msg.pose.at(link_number).position.z);
+ pls_msg.set_allocated_pos(pos);
+ }
- if (export_orientation) {
- panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_Orientation();
- orient->set_orientationw(msg.pose.at(link_number).orientation.w);
- orient->set_orientationx(msg.pose.at(link_number).orientation.x);
- orient->set_orientationy(msg.pose.at(link_number).orientation.y);
- orient->set_orientationz(msg.pose.at(link_number).orientation.z);
- pls_msg.set_allocated_orient(orient);
- }
+ if (export_orientation) {
+ auto *orient = new panda::PandaLinkState_Orientation();
+ orient->set_orientationw(msg.pose.at(link_number).orientation.w);
+ orient->set_orientationx(msg.pose.at(link_number).orientation.x);
+ orient->set_orientationy(msg.pose.at(link_number).orientation.y);
+ orient->set_orientationz(msg.pose.at(link_number).orientation.z);
+ pls_msg.set_allocated_orient(orient);
+ }
- if (export_twist_linear) {
- panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear();
- tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
- tl->set_twistlineary(msg.twist.at(link_number).linear.y);
- tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
- pls_msg.set_allocated_tl(tl);
- }
+ if (export_twist_linear) {
+ auto *tl = new panda::PandaLinkState_TwistLinear();
+ tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
+ tl->set_twistlineary(msg.twist.at(link_number).linear.y);
+ tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
+ pls_msg.set_allocated_tl(tl);
+ }
- if (export_twist_angular) {
- panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular();
- ta->set_twistangularx(msg.twist.at(link_number).angular.x);
- ta->set_twistangulary(msg.twist.at(link_number).angular.y);
- ta->set_twistangularz(msg.twist.at(link_number).angular.z);
- pls_msg.set_allocated_ta(ta);
- }
+ if (export_twist_angular) {
+ auto *ta = new panda::PandaLinkState_TwistAngular();
+ ta->set_twistangularx(msg.twist.at(link_number).angular.x);
+ ta->set_twistangulary(msg.twist.at(link_number).angular.y);
+ ta->set_twistangularz(msg.twist.at(link_number).angular.z);
+ pls_msg.set_allocated_ta(ta);
+ }
- messages.push_back(pls_msg);
- } else {
- ROS_INFO_STREAM_NAMED("MqttToRosNode", "Skipping the status of link " << name << ".");
+ messages.push_back(pls_msg);
+ } else {
+ ROS_INFO_STREAM_NAMED("MqttToRosNode", "Skipping the status of link " << name << ".");
+ }
}
+
+ return messages;
}
- return messages;
-}
+ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
-void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
+ if (mqttUtil->ensureConnection()) {
+ try {
+ for (const panda::PandaLinkState& pls_msg : buildLinkStateMessages(msg)) {
- if (mqttUtil->ensureConnection()) {
- try {
- for (panda::PandaLinkState pls_msg : buildLinkStateMessages(msg)) {
+ std::string mqtt_msg;
+ if (!pls_msg.SerializeToString(&mqtt_msg)) {
+ ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Serialization of incoming MQTT message has failed.");
+ }
- std::string mqtt_msg;
- if (!pls_msg.SerializeToString(&mqtt_msg)) {
- ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Serialization of incoming MQTT message has failed.");
+ auto pubmsg = mqtt::make_message(pls_msg.name(), mqtt_msg);
+ pubmsg->set_qos(QOS);
+ mqttUtil->getClient().publish(pubmsg);
}
-
- auto pubmsg = mqtt::make_message(pls_msg.name(), mqtt_msg);
- pubmsg->set_qos(QOS);
- mqttUtil->getClient().publish(pubmsg);
}
+ catch (const mqtt::exception &exc) {
+ ROS_ERROR_STREAM("Unable to publish robot state message. " << exc.what());
+ return;
+ }
+ } else {
+ ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages.");
}
- // happens at lib paho sometimes when multiple instances of an mqtt-client are publishing at the same time
- catch (const mqtt::exception &exc) {
- ROS_ERROR_STREAM("RSP: " << exc.what());
- return;
- }
- } else {
- ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Not connected! Unable to listen to messages.");
}
-}
-void providerCallback(const gazebo_msgs::LinkStates &msg) {
+ void providerCallback(const gazebo_msgs::LinkStates &msg) {
- // TODO the rate should not be depending on the link state rate, but rather be a time or frequency!!!!
+ static int current_redirect = 0;
- ros::NodeHandle n("panda_mqtt_connector");
- int message_redirect_rate;
- if (!n.getParam("data_publish_rate", message_redirect_rate)) {
- message_redirect_rate = 200; // fallback
+ // TODO the rate should not be depending on the link state rate, but rather be a time or frequency!!!!
+
+ ros::NodeHandle n("panda_mqtt_connector");
+ int message_redirect_rate;
+ if (!n.getParam("data_publish_rate", message_redirect_rate)) {
+ message_redirect_rate = 200; // fallback
+ }
+
+ if (current_redirect == 0) {
+ if (export_to_log) {
+ exportMessageToLog(msg);
+ }
+ sendMqttMessages(msg);
+ }
+ current_redirect = (current_redirect + 1) % message_redirect_rate;
}
- if (current_redirect == 0) {
- if (export_to_log) {
- exportMessageToLog(msg);
+ void readTopicList(const ros::NodeHandle &n) {
+ const std::vector<std::string> elementTypes{"/joints/", "/end_effectors/"};
+ std::vector<std::string> parameterNames;
+ n.getParamNames(parameterNames);
+ std::set<std::string> groups{};
+ for (const auto &elementType : elementTypes) {
+ // get the groups
+ for (const auto ¶m : parameterNames) {
+ std::string prefix{n.getNamespace() + elementType};
+ if (param.rfind(prefix, 0) == 0) {
+ std::string rest{param.substr(prefix.size())};
+ std::string element{rest.substr(0, rest.rfind('/'))};
+ groups.insert(element);
+ }
+ }
+
+ // get the elements in the group
+ for (const auto &group: groups) {
+ std::map<std::string, std::string> element_topics{};
+ std::string key{"/panda_mqtt_connector" + elementType + group};
+ if (!n.getParam(key, element_topics)) {
+ ROS_ERROR_STREAM("Unable to retrieve value for " << key);
+ }
+ for (const auto &pair : element_topics) {
+ std::string mqttTopic{group + "/" + pair.first};
+ topics[pair.second] = mqttTopic;
+ }
+ }
+ }
+
+ ROS_INFO_STREAM("Publishing the state of " << topics.size() << " ROS elements to MQTT.");
+ for (const auto &pair : topics) {
+ ROS_INFO_STREAM("ROS element: '" << pair.first << "' \t\tMQTT topic: '" << pair.second << "'");
}
- sendMqttMessages(msg);
}
- current_redirect = (current_redirect + 1) % message_redirect_rate;
}
+
int main(int argc, char **argv) {
ros::init(argc, argv, "robot_state_provider");
ros::NodeHandle n("panda_mqtt_connector");
@@ -224,11 +263,13 @@ int main(int argc, char **argv) {
return -1;
}
- mqttUtil = new MqttUtil(CLIENT_ID, server);
+ robot_state_provider::readTopicList(n);
+
+ robot_state_provider::mqttUtil = new MqttUtil(robot_state_provider::CLIENT_ID, server);
- mqttUtil->connect();
+ robot_state_provider::mqttUtil->connect();
- ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
+ ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, robot_state_provider::providerCallback);
ros::spin();
return 0;