From c714af16ff52f3bf8f7909177a0b7ff99a12f04b Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Thu, 13 Jun 2019 13:47:56 +0200
Subject: [PATCH] added a publisher for the tcp pose

---
 .../ur_rtde_driver/ros/hardware_interface.h     | 11 +++++++++++
 ur_rtde_driver/src/ros/hardware_interface.cpp   | 17 +++++++++++++++++
 2 files changed, 28 insertions(+)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
index c46cd78..dd620a6 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
@@ -33,6 +33,7 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
 #include <std_msgs/Bool.h>
+#include <realtime_tools/realtime_publisher.h>
 #include "tf2_msgs/TFMessage.h"
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
@@ -85,6 +86,14 @@ protected:
    * \param timestamp Timestamp of read data
    */
   void extractToolPose(const ros::Time& timestamp);
+
+  /*!
+   * \brief Publishes the tool pose to the tf system
+   *
+   * Requires extractToolPose() to be run first.
+   */
+  void publishPose();
+
   std::unique_ptr<UrDriver> ur_driver_;
 
   hardware_interface::JointStateInterface js_interface_;
@@ -109,6 +118,8 @@ protected:
   double speed_scaling_combined_;
   std::vector<std::string> joint_names_;
 
+  std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> tcp_pose_pub_;
+
   uint32_t runtime_state_;
   bool position_controller_running_;
 
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 5373b2f..9de6c74 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -181,6 +181,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   registerInterface(&speedsc_interface_);
   registerInterface(&fts_interface_);
 
+  tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
+
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
 
   return true;
@@ -230,6 +232,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
     // Transform fts measurements to tool frame
     extractToolPose(time);
     transformForceTorque();
+    publishPose();
 
     // pausing state follows runtime state when pausing
     if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))
@@ -376,4 +379,18 @@ void HardwareInterface ::extractToolPose(const ros::Time& timestamp)
 
   tcp_transform_.transform.rotation = tf2::toMsg(rotation);
 }
+
+void HardwareInterface ::publishPose()
+{
+  if (tcp_pose_pub_)
+  {
+    if (tcp_pose_pub_->trylock())
+    {
+      tcp_pose_pub_->msg_.transforms.clear();
+      tcp_pose_pub_->msg_.transforms.push_back(tcp_transform_);
+      tcp_pose_pub_->unlockAndPublish();
+    }
+  }
+}
+
 }  // namespace ur_driver