Select Git revision
RobotStateProvider.cpp
Sebastian Ebert authored
RobotStateProvider.cpp 8.16 KiB
//
// Created by sebastian on 31.03.20.
//
#include "ros/ros.h"
#include <gazebo_msgs/LinkStates.h>
#include <mqtt/client.h>
#include "string.h"
#include "linkstate.pb.h"
#include "mem_persistence.cpp"
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
*/
std::string topics[11] = {"panda_link_0", "panda_link_1", "panda_link_2", "panda_link_3",
"panda_link_4", "panda_link_5", "panda_link_6", "panda_link_7",
"panda_link_8", "panda_link_9", "panda_link_10"};
bool mqttSetup = false;
bool isInitialized = false;
const int QOS = 1;
int current_redirect = 0;
mem_persistence persist;
mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt", &persist);
/*
* logs to its own file in /.ros/logs (configured in launch-file)
*/
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;
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);
ROS_INFO_STREAM("<<< LINK NAMES >>>");
for (int i = 0; i < msg.name.size(); i++) {
ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
}
if (export_position) {
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);
}
}
if (export_orientation) {
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);
}
}
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);
}
}
}
panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg, int link_number) {
bool export_position = false;
bool export_orientation = false;
bool export_twist_linear = false;
bool export_twist_angular = false;
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);
//std::cout << "Building LinkStateMessage with pos: " << export_position << " ori: " << export_orientation << " twistlin: "
// << export_twist_linear << " twistang: " << export_twist_angular << std::endl;
panda::PandaLinkState pls_msg;
pls_msg.set_name(msg.name.at(link_number));
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_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_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_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);
}
return pls_msg;
}
void setupMqtt() {
if (!mqttSetup) {
std::cout << "MQTT: Initializing." << std::endl;
mqtt::connect_options connOpts;
connOpts.set_keep_alive_interval(20);
connOpts.set_clean_session(true);
std::cout << "MQTT: Initialized." << std::endl;
std::cout << "MQTT: Connecting." << std::endl;
client.connect(connOpts);
std::cout << "MQTT: Connected." << std::endl;
mqttSetup = true;
}
}
void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
setupMqtt();
try {
for (int i = 0; i < msg.name.size(); i++) {
panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
std::string mqtt_msg;
pls_msg.SerializeToString(&mqtt_msg);
//std::cout << "SENDING MESSAGE TO " << topics[i] << std::endl;
auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
pubmsg->set_qos(QOS);
client.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());
// causes automatic reconnect
mqttSetup = false;
return;
}
}
void providerCallback(const gazebo_msgs::LinkStates &msg) {
ros::NodeHandle n;
int message_redirect_rate = 200;
n.getParam("data_publish_rate", message_redirect_rate);
if (current_redirect != 0 && current_redirect != message_redirect_rate) {
// std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
current_redirect++;
return;
}
if (current_redirect == 0 || current_redirect == message_redirect_rate) {
//std::cout << "Sending LinkStates with rate: " << message_redirect_rate << std::endl;
if (export_to_log) { exportMessageToLog(msg);
}
sendMqttMessages(msg);
if (current_redirect == message_redirect_rate) {
current_redirect = 1;
} else {
current_redirect++;
}
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "robot_state_provider");
ros::NodeHandle n;
n.getParam("ready", isInitialized);
while (ros::ok()) {
// make sure the robot is initialized
while (!isInitialized) {
//std::cout << "Waiting" << std::endl;
n.getParam("tud_planner_ready", isInitialized);
}
ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
ros::spin();
}
// disconnect from mqtt
client.disconnect();
return 0;
}