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
Hide whitespace changes
Inline Side-by-side
config/config.yaml
+ 14
− 12
View file @ 902a67c6
panda_mqtt_connector: panda_mqtt_connector:
server: "tcp://localhost:1883" server: "tcp://localhost:1883"
topics: topics:
- robotConfig: "robotconfig" - robotConfig: "robotconfig"
- dataConfig: "dataconfig" - dataConfig: "dataconfig"
zone_size: 0.5 zone_size: 0.5
zones: zones:
- "1 1" - "1 1"
- "-1 -1 1" - "-1 -1 1"
joints: joints:
- panda/Joint0: "panda::panda_link0" panda:
- panda/Joint1: "panda::panda_link1" Joint0: "panda::panda_link0"
- panda/Joint2: "panda::panda_link2" Joint1: "panda::panda_link1"
- panda/Joint3: "panda::panda_link3" Joint2: "panda::panda_link2"
- panda/Joint4: "panda::panda_link4" Joint3: "panda::panda_link3"
- panda/Joint5: "panda::panda_link5" Joint4: "panda::panda_link4"
- panda/Joint6: "panda::panda_link6" Joint5: "panda::panda_link5"
- panda/LeftFinger: "panda::panda_leftfinger" Joint6: "panda::panda_link6"
- panda/RightFinger: "panda::panda_rightfinger" LeftFinger: "panda::panda_leftfinger"
RightFinger: "panda::panda_rightfinger"
end_effectors: end_effectors:
- panda/EndEffector: "panda::panda_link7" panda:
\ No newline at end of file EndEffector: "panda::panda_link7"
src/RobotStateProvider.cpp
+ 192
− 151
View file @ 902a67c6
// /** @file RobotStateProvider.cpp
// Created by sebastian on 31.03.20. *
// * @author Sebastian Ebert
* @author Johannes Mey
* @date 31.03.20
*/
#include "ros/ros.h" #include "ros/ros.h"
#include <gazebo_msgs/LinkStates.h> #include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h> #include <mqtt/client.h>
...@@ -9,211 +13,246 @@ ...@@ -9,211 +13,246 @@
#include "util/MqttUtil.h" #include "util/MqttUtil.h"
bool export_to_log = false; namespace robot_state_provider {
bool export_to_log = false;
/* /**
* mqtt-topics for all links * A map from the ROS element name to an MQTT topic
* ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
*/ */
std::set <std::string> topics = {"ground_plane::link", "panda::panda_link0", "panda::panda_link1", std::map<std::string, std::string> topics{};
"panda::panda_link2", "panda::panda_link3", "panda::panda_link4",
"panda::panda_link5", "panda::panda_link6", "panda::panda_link7",
"panda::panda_leftfinger", "panda::panda_rightfinger"};
bool isInitialized = false;
const int QOS = 1; constexpr int QOS = 0;
int current_redirect = 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) * 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_position = false;
bool export_orientation = false; bool export_orientation = false;
bool export_twist_linear = false; bool export_twist_linear = false;
bool export_twist_angular = false; bool export_twist_angular = false;
ros::NodeHandle n; ros::NodeHandle n;
n.getParam("data_enable_position", export_position); n.getParam("data_enable_position", export_position);
n.getParam("data_enable_orientation", export_orientation); n.getParam("data_enable_orientation", export_orientation);
n.getParam("data_enable_twist_linear", export_twist_linear); n.getParam("data_enable_twist_linear", export_twist_linear);
n.getParam("data_enable_twist_angular", export_twist_angular); 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++) { for (int i = 0; i < msg.name.size(); i++) {
ROS_INFO_STREAM("[" << i << "] " << msg.name.at(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++) { for (int i = 0; i < msg.pose.size(); i++) {
ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x); 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 << "] [y] " << msg.pose.at(i).position.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z); 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++) { for (int i = 0; i < msg.pose.size(); i++) {
ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w); 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 << "] [x] " << msg.pose.at(i).orientation.x);
ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y); ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z); ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
}
} }
}
if (export_twist_linear) { if (export_twist_linear) {
ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>"); ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
for (int i = 0; i < msg.twist.size(); i++) { for (int i = 0; i < msg.twist.size(); i++) {
ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x); 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 << "] [y] " << msg.twist.at(i).linear.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z); ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
}
} }
}
if (export_twist_angular) { if (export_twist_angular) {
ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>"); ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
for (int i = 0; i < msg.twist.size(); i++) { for (int i = 0; i < msg.twist.size(); i++) {
ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x); 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 << "] [y] " << msg.twist.at(i).angular.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z); 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_position = false;
bool export_orientation = false; bool export_orientation = false;
bool export_twist_linear = false; bool export_twist_linear = false;
bool export_twist_angular = 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)) { if (!n.getParam("data_enable_position", export_position)) {
export_position = false; export_position = false;
} }
if (!n.getParam("data_enable_orientation", export_orientation)) { if (!n.getParam("data_enable_orientation", export_orientation)) {
export_orientation = false; export_orientation = false;
} }
if (!n.getParam("data_enable_twist_linear", export_twist_linear)) { if (!n.getParam("data_enable_twist_linear", export_twist_linear)) {
export_twist_linear = false; export_twist_linear = false;
} }
if (!n.getParam("data_enable_twist_angular", export_twist_angular)) { if (!n.getParam("data_enable_twist_angular", export_twist_angular)) {
export_twist_angular = false; export_twist_angular = false;
} }
std::vector <panda::PandaLinkState> messages{}; std::vector<panda::PandaLinkState> messages{};
std::vector <std::string> names{msg.name}; std::vector<std::string> names{msg.name};
for (int link_number = 0; link_number < names.size(); ++link_number) { for (int link_number = 0; link_number < names.size(); ++link_number) {
std::string name = names[link_number]; std::string name = names[link_number];
if (topics.find(name) != topics.end()) { 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) { 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_positionx(msg.pose.at(link_number).position.x);
pos->set_positiony(msg.pose.at(link_number).position.y); pos->set_positiony(msg.pose.at(link_number).position.y);
pos->set_positionz(msg.pose.at(link_number).position.z); pos->set_positionz(msg.pose.at(link_number).position.z);
pls_msg.set_allocated_pos(pos); pls_msg.set_allocated_pos(pos);
} }
if (export_orientation) { 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_orientationw(msg.pose.at(link_number).orientation.w);
orient->set_orientationx(msg.pose.at(link_number).orientation.x); orient->set_orientationx(msg.pose.at(link_number).orientation.x);
orient->set_orientationy(msg.pose.at(link_number).orientation.y); orient->set_orientationy(msg.pose.at(link_number).orientation.y);
orient->set_orientationz(msg.pose.at(link_number).orientation.z); orient->set_orientationz(msg.pose.at(link_number).orientation.z);
pls_msg.set_allocated_orient(orient); pls_msg.set_allocated_orient(orient);
} }
if (export_twist_linear) { 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_twistlinearx(msg.twist.at(link_number).linear.x);
tl->set_twistlineary(msg.twist.at(link_number).linear.y); tl->set_twistlineary(msg.twist.at(link_number).linear.y);
tl->set_twistlinearz(msg.twist.at(link_number).linear.z); tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
pls_msg.set_allocated_tl(tl); pls_msg.set_allocated_tl(tl);
} }
if (export_twist_angular) { 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_twistangularx(msg.twist.at(link_number).angular.x);
ta->set_twistangulary(msg.twist.at(link_number).angular.y); ta->set_twistangulary(msg.twist.at(link_number).angular.y);
ta->set_twistangularz(msg.twist.at(link_number).angular.z); ta->set_twistangularz(msg.twist.at(link_number).angular.z);
pls_msg.set_allocated_ta(ta); pls_msg.set_allocated_ta(ta);
} }
messages.push_back(pls_msg); messages.push_back(pls_msg);
} else { } else {
ROS_INFO_STREAM_NAMED("MqttToRosNode", "Skipping the status of link " << name << "."); 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()) { std::string mqtt_msg;
try { if (!pls_msg.SerializeToString(&mqtt_msg)) {
for (panda::PandaLinkState pls_msg : buildLinkStateMessages(msg)) { ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Serialization of incoming MQTT message has failed.");
}
std::string mqtt_msg; auto pubmsg = mqtt::make_message(pls_msg.name(), mqtt_msg);
if (!pls_msg.SerializeToString(&mqtt_msg)) { pubmsg->set_qos(QOS);
ROS_ERROR_STREAM_NAMED("MqttToRosNode", "Serialization of incoming MQTT message has failed."); 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"); // TODO the rate should not be depending on the link state rate, but rather be a time or frequency!!!!
int message_redirect_rate;
if (!n.getParam("data_publish_rate", message_redirect_rate)) { ros::NodeHandle n("panda_mqtt_connector");
message_redirect_rate = 200; // fallback 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) { void readTopicList(const ros::NodeHandle &n) {
if (export_to_log) { const std::vector<std::string> elementTypes{"/joints/", "/end_effectors/"};
exportMessageToLog(msg); 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 << "'");
} }
sendMqttMessages(msg);
} }
current_redirect = (current_redirect + 1) % message_redirect_rate;
} }
int main(int argc, char **argv) { int main(int argc, char **argv) {
ros::init(argc, argv, "robot_state_provider"); ros::init(argc, argv, "robot_state_provider");
ros::NodeHandle n("panda_mqtt_connector"); ros::NodeHandle n("panda_mqtt_connector");
...@@ -224,11 +263,13 @@ int main(int argc, char **argv) { ...@@ -224,11 +263,13 @@ int main(int argc, char **argv) {
return -1; 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(); ros::spin();
return 0; return 0;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment