update state provider to use tf2 state

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"

src/RobotStateProvider.cpp

 /** @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,70 +31,19 @@ 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.");
+          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());
         return;
@@ -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;
+
 }