testRT_comm.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('ur_driver')
00003 import time, sys, threading, math
00004 import copy
00005 import datetime
00006 import socket, select
00007 import struct
00008 import traceback, code
00009 import SocketServer
00010 
00011 import rospy
00012 
00013 from sensor_msgs.msg import JointState
00014 from ur_driver.deserializeRT import RobotStateRT
00015 from ur_msgs.msg import *
00016 
00017 
00018 joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
00019                'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
00020 joint_offsets = {}
00021 
00022 
00023 def __on_packet(buf):
00024     stateRT = RobotStateRT.unpack(buf)
00025     
00026     msg = RobotStateRTMsg()
00027     msg.time = stateRT.time
00028     msg.q_target = stateRT.q_target
00029     msg.qd_target = stateRT.qd_target
00030     msg.qdd_target = stateRT.qdd_target
00031     msg.i_target = stateRT.i_target
00032     msg.m_target = stateRT.m_target
00033     msg.q_actual = stateRT.q_actual
00034     msg.qd_actual = stateRT.qd_actual
00035     msg.i_actual = stateRT.i_actual
00036     msg.tool_acc_values = stateRT.tool_acc_values
00037     msg.tcp_force = stateRT.tcp_force
00038     msg.tool_vector = stateRT.tool_vector
00039     msg.tcp_speed = stateRT.tcp_speed
00040     msg.digital_input_bits = stateRT.digital_input_bits
00041     msg.motor_temperatures = stateRT.motor_temperatures
00042     msg.controller_timer = stateRT.controller_timer
00043     msg.test_value = stateRT.test_value
00044     msg.robot_mode = stateRT.robot_mode
00045     msg.joint_modes = stateRT.joint_modes
00046     pub_robot_stateRT.publish(msg)       
00047     
00048 
00049     msg = JointState()
00050     msg.header.stamp = rospy.get_rostime()
00051     msg.header.frame_id = "From real-time state data"
00052     msg.name = joint_names
00053     msg.position = [0.0] * 6
00054     for i, q in enumerate(stateRT.q_actual):
00055         msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
00056     msg.velocity = stateRT.qd_actual
00057     msg.effort = [0]*6
00058     pub_joint_statesRT.publish(msg)
00059 
00060 
00061 
00062 
00063 def main():
00064     rospy.init_node('testRT_comm', disable_signals=True)
00065     
00066     global pub_joint_statesRT
00067     pub_joint_statesRT = rospy.Publisher('joint_statesRT', JointState)
00068     global pub_robot_stateRT
00069     pub_robot_stateRT = rospy.Publisher('robot_stateRT', RobotStateRTMsg)
00070     
00071     
00072     
00073     robot_hostname = '192.168.0.42'
00074     rt_port = 30003
00075     
00076     rt_socket = socket.create_connection((robot_hostname, rt_port))
00077     buf = ""
00078     
00079     while not rospy.is_shutdown():
00080         more = rt_socket.recv(4096)
00081         if more:
00082             buf = buf + more
00083 
00084             # Attempts to extract a packet
00085             packet_length = struct.unpack_from("!i", buf)[0]
00086             print("PacketLength: ", packet_length, "; BufferSize: ", len(buf))
00087             if len(buf) >= packet_length:
00088                 packet, buf = buf[:packet_length], buf[packet_length:]
00089                 __on_packet(packet)
00090         else:
00091             print("There is no more...")
00092     
00093     rt_socket.close()
00094 
00095 
00096 
00097 
00098 if __name__ == '__main__': main()


ur_driver
Author(s): Stuart Glaser, Shaun Edwards , Felix Messmer
autogenerated on Fri Aug 28 2015 13:31:30