diff --git a/config/config.yaml b/config/config.yaml
index 98d68fee3e7f4c542f96c6e04d1c75859c4856ec..cb1f77a4e39657b5e449de81e32f5be84e6e976e 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -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"
diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp
index 25b480961b364bb71c7af8d03753173e47fcdd6b..21970e5ad13c56b16b1e7536d5e5243d86e10c4f 100644
--- a/src/RobotStateProvider.cpp
+++ b/src/RobotStateProvider.cpp
@@ -1,14 +1,16 @@
/** @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;
+
}