From e7d6248b5fe8fb15bd6fc5bd0ec46e10b6658f28 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Mon, 26 Mar 2012 13:26:11 -0700
Subject: [PATCH] Publishing joint states. Successfully showing up in rviz
---
ur5_driver/driver.py | 21 +++++++++++++++++++--
ur5_driver/manifest.xml | 2 ++
2 files changed, 21 insertions(+), 2 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 9473d6b..602f330 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -4,6 +4,8 @@ import time, sys, threading
import socket
import struct
import SocketServer
+import rospy
+from sensor_msgs.msg import JointState
HOSTNAME='ur-xx'
HOSTNAME="10.0.1.20"
@@ -14,6 +16,11 @@ MSG_QUIT = 2
MSG_JOINT_STATES = 3
MULT_jointstate = 1000.0
+JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+ 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+
+pub_joint_states = rospy.Publisher('joint_states', JointState)
+
class EOF(Exception): pass
# Only handles JOINT_STATES messages.
@@ -55,6 +62,14 @@ class StateTCPHandler(SocketServer.BaseRequestHandler):
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
print "Joint state:", state
+
+ msg = JointState()
+ msg.header.stamp = rospy.get_rostime()
+ msg.name = JOINT_NAMES
+ msg.position = state[:6]
+ msg.velocity = state[6:12]
+ msg.effort = state[12:18]
+ pub_joint_states.publish(msg)
else:
raise Exception("Unknown message type: %i" % mtype)
@@ -120,6 +135,7 @@ class TCPServer(SocketServer.TCPServer):
sock = None
def main():
global sock
+ rospy.init_node('ur5_driver', disable_signals=True)
# Sends the program to the robot
sock = socket.create_connection((HOSTNAME, PORT))
@@ -127,10 +143,11 @@ def main():
sock.sendall(fin.read())
server = TCPServer(("", 50001), CommanderTCPHandler)
- server2 = TCPServer(("", 50002), StateTCPHandler)
+ server_state = TCPServer(("", 50002), StateTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.handle_request)
- thread_state = threading.Thread(name="StateHandler", target=server2.handle_request)
+ thread_state = threading.Thread(name="StateHandler", target=server_state.handle_request)
+ thread_commander.daemon = True
thread_state.daemon = True
thread_commander.start()
thread_state.start()
diff --git a/ur5_driver/manifest.xml b/ur5_driver/manifest.xml
index cb5be27..4917f4b 100644
--- a/ur5_driver/manifest.xml
+++ b/ur5_driver/manifest.xml
@@ -9,6 +9,8 @@
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ur5_driver</url>
+ <depend package="rospy" />
+
</package>
--
GitLab