Skip to content
Snippets Groups Projects
Commit 902a67c6 authored by Johannes Mey's avatar Johannes Mey
Browse files

read topics to be published from config file

parent 75917759
Branches
No related tags found
No related merge requests found
......@@ -8,14 +8,16 @@ panda_mqtt_connector:
- "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"
//
// 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,23 +13,18 @@
#include "util/MqttUtil.h"
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;
......@@ -125,10 +124,11 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
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();
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);
......@@ -136,7 +136,7 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
}
if (export_orientation) {
panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_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);
......@@ -145,7 +145,7 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
}
if (export_twist_linear) {
panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear();
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);
......@@ -153,7 +153,7 @@ std::vector <panda::PandaLinkState> buildLinkStateMessages(const gazebo_msgs::Li
}
if (export_twist_angular) {
panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular();
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);
......@@ -173,7 +173,7 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
if (mqttUtil->ensureConnection()) {
try {
for (panda::PandaLinkState pls_msg : buildLinkStateMessages(msg)) {
for (const panda::PandaLinkState& pls_msg : buildLinkStateMessages(msg)) {
std::string mqtt_msg;
if (!pls_msg.SerializeToString(&mqtt_msg)) {
......@@ -185,9 +185,8 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
mqttUtil->getClient().publish(pubmsg);
}
}
// 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());
ROS_ERROR_STREAM("Unable to publish robot state message. " << exc.what());
return;
}
} else {
......@@ -197,6 +196,8 @@ void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
void providerCallback(const gazebo_msgs::LinkStates &msg) {
static int current_redirect = 0;
// 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");
......@@ -214,6 +215,44 @@ void providerCallback(const gazebo_msgs::LinkStates &msg) {
current_redirect = (current_redirect + 1) % message_redirect_rate;
}
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 &param : 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 << "'");
}
}
}
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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment