From 3ff1c703ba7dc72297b2d3b80b3b91fa2be59be2 Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Thu, 25 Sep 2014 14:49:51 +0000
Subject: [PATCH] script for testing communication with real-time communication
interface
---
ur_driver/src/ur_driver/testRT_comm.py | 102 +++++++++++++++++++++++++
1 file changed, 102 insertions(+)
create mode 100755 ur_driver/src/ur_driver/testRT_comm.py
diff --git a/ur_driver/src/ur_driver/testRT_comm.py b/ur_driver/src/ur_driver/testRT_comm.py
new file mode 100755
index 0000000..8f2f8cc
--- /dev/null
+++ b/ur_driver/src/ur_driver/testRT_comm.py
@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('ur_driver')
+import time, sys, threading, math
+import copy
+import datetime
+import socket, select
+import struct
+import traceback, code
+import SocketServer
+
+import rospy
+
+from sensor_msgs.msg import JointState
+from ur_driver.deserializeRT import RobotStateRT
+from ur_msgs.msg import *
+
+
+joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+ 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+joint_offsets = {}
+
+
+def __on_packet(buf):
+ stateRT = RobotStateRT.unpack(buf)
+
+ msg = RobotStateRTMsg()
+ msg.time = stateRT.time
+ msg.q_target = stateRT.q_target
+ msg.qd_target = stateRT.qd_target
+ msg.qdd_target = stateRT.qdd_target
+ msg.i_target = stateRT.i_target
+ msg.m_target = stateRT.m_target
+ msg.q_actual = stateRT.q_actual
+ msg.qd_actual = stateRT.qd_actual
+ msg.i_actual = stateRT.i_actual
+ msg.tool_acc_values = stateRT.tool_acc_values
+ msg.tcp_force = stateRT.tcp_force
+ msg.tool_vector = stateRT.tool_vector
+ msg.tcp_speed = stateRT.tcp_speed
+ msg.digital_input_bits = stateRT.digital_input_bits
+ msg.motor_temperatures = stateRT.motor_temperatures
+ msg.controller_timer = stateRT.controller_timer
+ msg.test_value = stateRT.test_value
+ msg.robot_mode = stateRT.robot_mode
+ msg.joint_modes = stateRT.joint_modes
+ pub_robot_stateRT.publish(msg)
+
+
+ msg = JointState()
+ msg.header.stamp = rospy.get_rostime()
+ msg.header.frame_id = "From real-time state data"
+ msg.name = joint_names
+ msg.position = [0.0] * 6
+ for i, q in enumerate(stateRT.q_actual):
+ msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
+ msg.velocity = stateRT.qd_actual
+ msg.effort = [0]*6
+ pub_joint_statesRT.publish(msg)
+
+
+
+
+def main():
+ rospy.init_node('testRT_comm', disable_signals=True)
+
+ global pub_joint_statesRT
+ pub_joint_statesRT = rospy.Publisher('joint_statesRT', JointState)
+ global pub_robot_stateRT
+ pub_robot_stateRT = rospy.Publisher('robot_stateRT', RobotStateRTMsg)
+
+
+
+ robot_hostname = '192.168.0.42'
+ rt_port = 30003
+
+ rt_socket = socket.create_connection((robot_hostname, rt_port))
+ buf = ""
+
+ while not rospy.is_shutdown():
+ more = rt_socket.recv(4096)
+ if more:
+ buf = buf + more
+
+ # Attempts to extract a packet
+ packet_length = struct.unpack_from("!i", buf)[0]
+ print("PacketLength: ", packet_length, "; BufferSize: ", len(buf))
+ if len(buf) >= packet_length:
+ packet, buf = buf[:packet_length], buf[packet_length:]
+ __on_packet(packet)
+ else:
+ print("There is no more...")
+
+
+
+
+
+ rt_socket.close()
+
+
+
+
+if __name__ == '__main__': main()
--
GitLab