Implemented RT publishing

include/ur_modern_driver/ros/converter.h

@@ -0,0 +1,2 @@
+#pragma once
+

include/ur_modern_driver/ros/rt_publisher.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <cstdlib>
+#include <vector>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+
+#include "ur_modern_driver/ur/consumer.h"
+
+using namespace ros;
+
+const std::string JOINTS[] = {
+    "shoulder_pan_joint",
+    "shoulder_lift_joint",
+    "elbow_joint",
+    "wrist_1_joint",
+    "wrist_2_joint",
+    "wrist_3_joint"
+};
+
+class RTPublisher : public URRTPacketConsumer {
+private:
+    NodeHandle _nh;
+    Publisher _joint_pub;
+    Publisher _wrench_pub;
+    Publisher _tool_vel_pub; 
+    std::vector<std::string> _joint_names;
+    std::string _base_frame;
+
+    bool publish_joints(RTShared &packet, ros::Time &t);
+    bool publish_wrench(RTShared &packet, ros::Time &t);
+    bool publish_tool(RTShared &packet, ros::Time &t);
+
+    bool publish(RTShared &packet);
+
+public:
+    RTPublisher(std::string &joint_prefix, std::string &base_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)),
+        _base_frame(base_frame)
+    {
+        for(auto const& j : JOINTS) {
+            _joint_names.push_back(joint_prefix + j);
+        }
+    }
+
+    virtual bool consume(RTState_V1_6__7 &state);
+    virtual bool consume(RTState_V1_8 &state);
+    virtual bool consume(RTState_V3_0__1 &state);
+    virtual bool consume(RTState_V3_2__3 &state);
+
+    virtual void setup_consumer() { }
+    virtual void teardown_consumer() { }
+    virtual void stop_consumer() { }
+};

src/ros/rt_publisher.cpp

@@ -0,0 +1,74 @@
+#include "ur_modern_driver/ros/rt_publisher.h"
+
+bool RTPublisher::publish_joints(RTShared &packet, ros::Time &t) {
+	sensor_msgs::JointState joint_msg;
+    joint_msg.header.stamp = t;
+    joint_msg.name = _joint_names;
+
+    for(auto const& q : packet.q_actual) {
+        joint_msg.position.push_back(q);
+    }
+    for(auto const& qd : packet.qd_actual) {
+        joint_msg.velocity.push_back(qd);
+    }
+    for(auto const& i : packet.i_actual) {
+        joint_msg.effort.push_back(i);
+    }
+
+    _joint_pub.publish(joint_msg);
+
+    return true;
+}
+
+bool RTPublisher::publish_wrench(RTShared &packet, ros::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];
+    wrench_msg.wrench.torque.x = packet.tcp_force[3];
+    wrench_msg.wrench.torque.y = packet.tcp_force[4];
+    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) {
+    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;
+    tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
+    tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
+    tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
+
+    _tool_vel_pub.publish(tool_twist);
+
+    return true;
+}
+
+bool RTPublisher::publish(RTShared &packet) {
+    ros::Time time = ros::Time::now();
+    return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time);
+}
+
+bool RTPublisher::consume(RTState_V1_6__7 &state) {
+    return publish(state);
+}
+bool RTPublisher::consume(RTState_V1_8 &state) {
+    return publish(state);
+}
+bool RTPublisher::consume(RTState_V3_0__1 &state) {
+    return publish(state);
+}
+bool RTPublisher::consume(RTState_V3_2__3 &state) {
+    return publish(state);
+}