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

update state provider to use tf2 state

parent cbe3cda5
Branches
Tags
No related merge requests found
......@@ -15,18 +15,18 @@ panda_mqtt_connector:
- "-1 1"
parts:
panda:
Link0: "panda::panda_link0"
Link1: "panda::panda_link1"
Link2: "panda::panda_link2"
Link3: "panda::panda_link3"
Link4: "panda::panda_link4"
Link5: "panda::panda_link5"
Link6: "panda::panda_link6"
EndEffector: "panda::panda_link7"
LeftFinger: "panda::panda_leftfinger"
Link0: "panda_link0"
Link1: "panda_link1"
Link2: "panda_link2"
Link3: "panda_link3"
Link4: "panda_link4"
Link5: "panda_link5"
Link6: "panda_link6"
RightFinger: "panda_rightfinger"
LeftFinger: "panda_leftfinger"
end_effectors:
panda:
RightFinger: "panda::panda_rightfinger"
EndEffector: "panda_hand"
goal_poses:
- position: "0.4 0.4 0.3"
orientation: "1 1 0 0"
......
/** @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>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "robot_state.pb.h"
#include "util/MqttUtil.h"
......@@ -29,69 +31,18 @@ namespace robot_state_provider {
MqttUtil *mqttUtil = nullptr;
std::vector<robot::RobotState> buildLinkStateMessages(const gazebo_msgs::LinkStates &msg) {
std::vector<robot::RobotState> 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()) {
robot::RobotState pls_msg;
// set the name to the MQTT topic provided by the topics map
pls_msg.set_name(topics[name]);
auto *pos = new robot::RobotState_Position();
pos->set_x(msg.pose.at(link_number).position.x);
pos->set_y(msg.pose.at(link_number).position.y);
pos->set_z(msg.pose.at(link_number).position.z);
pls_msg.set_allocated_position(pos);
auto *orient = new robot::RobotState_Orientation();
orient->set_w(msg.pose.at(link_number).orientation.w);
orient->set_x(msg.pose.at(link_number).orientation.x);
orient->set_y(msg.pose.at(link_number).orientation.y);
orient->set_z(msg.pose.at(link_number).orientation.z);
pls_msg.set_allocated_orientation(orient);
auto *tl = new robot::RobotState_LinearTwist();
tl->set_x(msg.twist.at(link_number).linear.x);
tl->set_y(msg.twist.at(link_number).linear.y);
tl->set_z(msg.twist.at(link_number).linear.z);
pls_msg.set_allocated_linear_twist(tl);
auto *ta = new robot::RobotState_AngularTwist();
ta->set_x(msg.twist.at(link_number).angular.x);
ta->set_y(msg.twist.at(link_number).angular.y);
ta->set_z(msg.twist.at(link_number).angular.z);
pls_msg.set_allocated_angular_twist(ta);
messages.push_back(pls_msg);
} else {
ROS_DEBUG_STREAM("Skipping the status of link " << name << ".");
}
}
return messages;
}
void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
void sendMqttMessages(const robot::RobotState &pls_msg) {
if (mqttUtil->ensureConnection()) {
try {
for (const robot::RobotState &pls_msg : buildLinkStateMessages(msg)) {
std::string mqtt_msg;
if (!pls_msg.SerializeToString(&mqtt_msg)) {
ROS_ERROR_STREAM("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);
std::string mqtt_msg;
if (!pls_msg.SerializeToString(&mqtt_msg)) {
ROS_ERROR_STREAM("Serialization of MQTT message has failed.");
}
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());
......@@ -102,24 +53,6 @@ namespace robot_state_provider {
}
}
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");
int message_redirect_rate;
if (!n.getParam("data_publish_rate", message_redirect_rate)) {
message_redirect_rate = 200; // fallback
}
if (current_redirect == 0) {
sendMqttMessages(msg);
}
current_redirect = (current_redirect + 1) % message_redirect_rate;
}
void readTopicList(const ros::NodeHandle &n) {
const std::vector<std::string> elementTypes{"/parts/", "/end_effectors/"};
std::vector<std::string> parameterNames;
......@@ -164,7 +97,7 @@ int main(int argc, char **argv) {
std::string server;
if (!n.getParam("server", server)) {
ROS_ERROR("Could not get string value for panda_mqtt_connector/server from param server");
ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/server from param server");
return -1;
}
......@@ -174,8 +107,69 @@ int main(int argc, char **argv) {
robot_state_provider::mqttUtil->connect();
ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, robot_state_provider::providerCallback);
ros::spin();
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
int message_redirect_rate;
if (!n.getParam("data_publish_rate", message_redirect_rate)) {
message_redirect_rate = 200; // fallback
}
// map from ROS topic to ROS publisher
std::map<std::string, ros::Publisher> publishers{};
// advertise the ROS messages
for (const auto& topic : robot_state_provider::topics) {
publishers[topic.second] = n.advertise<geometry_msgs::PoseStamped>(topic.second, 10);
}
ros::Rate rate(message_redirect_rate);
while (n.ok()){
for (const auto& topic : robot_state_provider::topics) {
geometry_msgs::TransformStamped transformStamped;
try{
transformStamped = tfBuffer.lookupTransform("world", topic.first,ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(0.1).sleep();
continue;
}
// publish the ROS message
geometry_msgs::PoseStamped ros_msg;
ros_msg.header.stamp = transformStamped.header.stamp;
ros_msg.pose.position.x = transformStamped.transform.translation.x;
ros_msg.pose.position.y = transformStamped.transform.translation.y;
ros_msg.pose.position.z = transformStamped.transform.translation.z;
ros_msg.pose.orientation.w = transformStamped.transform.rotation.w;
ros_msg.pose.orientation.x = transformStamped.transform.rotation.x;
ros_msg.pose.orientation.y = transformStamped.transform.rotation.y;
ros_msg.pose.orientation.z = transformStamped.transform.rotation.z;
publishers[topic.second].publish(ros_msg);
// publish the MQTT message
robot::RobotState pls_msg;
// set the name to the MQTT topic provided by the topics map
pls_msg.set_name(topic.second);
auto *pos = new robot::RobotState_Position();
pos->set_x(transformStamped.transform.translation.x);
pos->set_y(transformStamped.transform.translation.y);
pos->set_z(transformStamped.transform.translation.z);
pls_msg.set_allocated_position(pos);
auto *orient = new robot::RobotState_Orientation();
orient->set_w(transformStamped.transform.rotation.w);
orient->set_x(transformStamped.transform.rotation.x);
orient->set_y(transformStamped.transform.rotation.y);
orient->set_z(transformStamped.transform.rotation.z);
pls_msg.set_allocated_orientation(orient);
robot_state_provider::sendMqttMessages(pls_msg);
}
rate.sleep();
}
return 0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment