Improved RTPublisher

include/ur_modern_driver/ros/rt_publisher.h

@@ -6,11 +6,15 @@
 #include <geometry_msgs/WrenchStamped.h>
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
+#include <sensor_msgs/Temperature.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
 #include <vector>
 
 #include "ur_modern_driver/ur/consumer.h"
 
 using namespace ros;
+using namespace tf;
 
 const std::string JOINTS[] = {
     "shoulder_pan_joint",
@@ -27,21 +31,28 @@ private:
     Publisher _joint_pub;
     Publisher _wrench_pub;
     Publisher _tool_vel_pub;
+    Publisher _joint_temperature_pub;
+    TransformBroadcaster _transform_broadcaster;
     std::vector<std::string> _joint_names;
     std::string _base_frame;
+    std::string _tool_frame;
 
-    bool publish_joints(RTShared& packet, ros::Time& t);
+    bool publish_joints(RTShared& packet, Time& t);
-    bool publish_wrench(RTShared& packet, ros::Time& t);
+    bool publish_wrench(RTShared& packet, Time& t);
-    bool publish_tool(RTShared& packet, ros::Time& t);
+    bool publish_tool(RTShared& packet, Time& t);
+    bool publish_transform(RTShared& packet, Time& t);
+    bool publish_temperature(RTShared& packet, Time& t);
 
     bool publish(RTShared& packet);
 
 public:
-    RTPublisher(std::string& joint_prefix, std::string& base_frame)
+    RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame)
         : _joint_pub(_nh.advertise<sensor_msgs::JointState>("joint_states", 1))
         , _wrench_pub(_nh.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
         , _tool_vel_pub(_nh.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
+        , _joint_temperature_pub(_nh.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
         , _base_frame(base_frame)
+        , _tool_frame(tool_frame)
     {
         for (auto const& j : JOINTS) {
             _joint_names.push_back(joint_prefix + j);

src/ros/rt_publisher.cpp

@@ -1,6 +1,6 @@
 #include "ur_modern_driver/ros/rt_publisher.h"
 
-bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
+bool RTPublisher::publish_joints(RTShared& packet, Time& t)
 {
     sensor_msgs::JointState joint_msg;
     joint_msg.header.stamp = t;
@@ -21,11 +21,10 @@ bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
     return true;
 }
 
-bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t)
+bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
 {
     geometry_msgs::WrenchStamped wrench_msg;
     wrench_msg.header.stamp = t;
-
     wrench_msg.wrench.force.x = packet.tcp_force[0];
     wrench_msg.wrench.force.y = packet.tcp_force[1];
     wrench_msg.wrench.force.z = packet.tcp_force[2];
@@ -34,18 +33,14 @@ bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t)
     wrench_msg.wrench.torque.z = packet.tcp_force[5];
 
     _wrench_pub.publish(wrench_msg);
-
     return true;
 }
 
-bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t)
+bool RTPublisher::publish_tool(RTShared& packet, Time& t)
 {
     geometry_msgs::TwistStamped tool_twist;
-
     tool_twist.header.stamp = t;
-
     tool_twist.header.frame_id = _base_frame;
-
     tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
     tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
     tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
@@ -54,14 +49,56 @@ bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t)
     tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
 
     _tool_vel_pub.publish(tool_twist);
+    return true;
+}
 
+bool RTPublisher::publish_transform(RTShared& packet, Time& t)
+{
+
+    auto tv = packet.tool_vector_actual;
+
+    Transform transform;
+    transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
+
+    //Create quaternion
+    Quaternion quat;
+
+    double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
+    if (angle < 1e-16) {
+        quat.setValue(0, 0, 0, 1);
+    } else {
+        quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
+    }
+
+    transform.setRotation(quat);
+
+    _transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
+
+    return true;
+}
+
+bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
+{
+    size_t len = _joint_names.size();
+    for (size_t i = 0; i < len; i++) {
+        sensor_msgs::Temperature msg;
+        msg.header.stamp = t;
+        msg.header.frame_id = _joint_names[i];
+        msg.temperature = packet.motor_temperatures[i];
+
+        _joint_temperature_pub.publish(msg);
+    }
     return true;
 }
 
 bool RTPublisher::publish(RTShared& packet)
 {
-    ros::Time time = ros::Time::now();
+    Time time = Time::now();
-    return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time);
+    return publish_joints(packet, time)
+        && publish_wrench(packet, time)
+        && publish_tool(packet, time)
+        && publish_transform(packet, time)
+        && publish_temperature(packet, time);
 }
 
 bool RTPublisher::consume(RTState_V1_6__7& state)