Skip to content
Snippets Groups Projects
Commit d567e892 authored by ipa-fxm's avatar ipa-fxm
Browse files

main commit: use ur socket protocols to reduce load on prog

parent 2f2a32ed
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ur_driver/prog
+ 88
− 84
View file @ d567e892
......@@ -38,85 +38,89 @@ def driverProg():
exit_critical
end
thread statePublisherThread():
def send_joint_state():
q = get_joint_positions()
qdot = get_joint_speeds()
tau = get_joint_torques()
enter_critical
socket_send_int(MSG_JOINT_STATES)
socket_send_int(floor(MULT_jointstate * q[0]))
socket_send_int(floor(MULT_jointstate * q[1]))
socket_send_int(floor(MULT_jointstate * q[2]))
socket_send_int(floor(MULT_jointstate * q[3]))
socket_send_int(floor(MULT_jointstate * q[4]))
socket_send_int(floor(MULT_jointstate * q[5]))
socket_send_int(floor(MULT_jointstate * qdot[0]))
socket_send_int(floor(MULT_jointstate * qdot[1]))
socket_send_int(floor(MULT_jointstate * qdot[2]))
socket_send_int(floor(MULT_jointstate * qdot[3]))
socket_send_int(floor(MULT_jointstate * qdot[4]))
socket_send_int(floor(MULT_jointstate * qdot[5]))
socket_send_int(floor(MULT_jointstate * tau[0]))
socket_send_int(floor(MULT_jointstate * tau[1]))
socket_send_int(floor(MULT_jointstate * tau[2]))
socket_send_int(floor(MULT_jointstate * tau[3]))
socket_send_int(floor(MULT_jointstate * tau[4]))
socket_send_int(floor(MULT_jointstate * tau[5]))
#socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
#socket_open(HOSTNAME, 50002)
send_joint_state()
while True:
send_joint_state()
sync()
end
sync()
end
thread wrench_statePublisherThread():
def send_wrench_state():
wrench = get_tcp_force()
enter_critical
socket_send_int(MSG_WRENCH)
socket_send_int(floor(MULT_wrench * wrench[0]))
socket_send_int(floor(MULT_wrench * wrench[1]))
socket_send_int(floor(MULT_wrench * wrench[2]))
socket_send_int(floor(MULT_wrench * wrench[3]))
socket_send_int(floor(MULT_wrench * wrench[4]))
socket_send_int(floor(MULT_wrench * wrench[5]))
exit_critical
end
send_wrench_state()
while True:
send_wrench_state()
sync()
end
sync()
end
thread io_statePublisherThread():
def send_io_state():
enter_critical
socket_send_int(MSG_GET_IO)
###socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31))
socket_send_int(1*get_flag(0) + 2*get_flag(1) + 4*get_flag(2) + 8*get_flag(3) + 16*get_flag(4) + 32*get_flag(5) + 64*get_flag(6) + 128*get_flag(7))
socket_send_int(1*get_flag(8) + 2*get_flag(9) + 4*get_flag(10) + 8*get_flag(11) + 16*get_flag(12) + 32*get_flag(13) + 64*get_flag(14) + 128*get_flag(15))
socket_send_int(1*get_flag(16) + 2*get_flag(17) + 4*get_flag(18) + 8*get_flag(19) + 16*get_flag(20) + 32*get_flag(21) + 64*get_flag(22) + 128*get_flag(23))
socket_send_int(1*get_flag(24) + 2*get_flag(25) + 4*get_flag(26) + 8*get_flag(27) + 16*get_flag(28) + 32*get_flag(29) + 64*get_flag(30) + 128*get_flag(31))
socket_send_int(get_analog_in(2) * MULT_analog)
socket_send_int(get_analog_in(3) * MULT_analog)
exit_critical
end
send_io_state()
while True:
send_io_state()
sync()
end
sync()
end
## For joint states the real-time communication interface connection is used
#thread statePublisherThread():
# def send_joint_state():
# q = get_joint_positions()
# qdot = get_joint_speeds()
# tau = get_joint_torques()
# enter_critical
# socket_send_int(MSG_JOINT_STATES)
# socket_send_int(floor(MULT_jointstate * q[0]))
# socket_send_int(floor(MULT_jointstate * q[1]))
# socket_send_int(floor(MULT_jointstate * q[2]))
# socket_send_int(floor(MULT_jointstate * q[3]))
# socket_send_int(floor(MULT_jointstate * q[4]))
# socket_send_int(floor(MULT_jointstate * q[5]))
# socket_send_int(floor(MULT_jointstate * qdot[0]))
# socket_send_int(floor(MULT_jointstate * qdot[1]))
# socket_send_int(floor(MULT_jointstate * qdot[2]))
# socket_send_int(floor(MULT_jointstate * qdot[3]))
# socket_send_int(floor(MULT_jointstate * qdot[4]))
# socket_send_int(floor(MULT_jointstate * qdot[5]))
# socket_send_int(floor(MULT_jointstate * tau[0]))
# socket_send_int(floor(MULT_jointstate * tau[1]))
# socket_send_int(floor(MULT_jointstate * tau[2]))
# socket_send_int(floor(MULT_jointstate * tau[3]))
# socket_send_int(floor(MULT_jointstate * tau[4]))
# socket_send_int(floor(MULT_jointstate * tau[5]))
# #socket_send_int(7895160) # Recognizable ".xxx" or 00787878
# exit_critical
# end
# #socket_open(HOSTNAME, 50002)
# send_joint_state()
# while True:
# send_joint_state()
# sync()
# end
# sync()
#end
## For tcp wrenches the real-time communication interface connection is used
#thread wrench_statePublisherThread():
# def send_wrench_state():
# wrench = get_tcp_force()
# enter_critical
# socket_send_int(MSG_WRENCH)
# socket_send_int(floor(MULT_wrench * wrench[0]))
# socket_send_int(floor(MULT_wrench * wrench[1]))
# socket_send_int(floor(MULT_wrench * wrench[2]))
# socket_send_int(floor(MULT_wrench * wrench[3]))
# socket_send_int(floor(MULT_wrench * wrench[4]))
# socket_send_int(floor(MULT_wrench * wrench[5]))
# exit_critical
# end
# send_wrench_state()
# while True:
# send_wrench_state()
# sync()
# end
# sync()
#end
#thread io_statePublisherThread():
# def send_io_state():
# enter_critical
# socket_send_int(MSG_GET_IO)
# socket_send_int(1*get_digital_in(0) + 2*get_digital_in(1) + 4 * get_digital_in(2) + 8 * get_digital_in(3) + 16 * get_digital_in(4) + 32 * get_digital_in(5) + 64 * get_digital_in(6) + 128 * get_digital_in(7) + 256 * get_digital_in(8) + 512 * get_digital_in(9))
# socket_send_int(1*get_digital_out(0) + 2*get_digital_out(1) + 4 * get_digital_out(2) + 8 * get_digital_out(3) + 16 * get_digital_out(4) + 32 * get_digital_out(5) + 64 * get_digital_out(6) + 128 * get_digital_out(7) + 256 * get_digital_out(8) + 512 * get_digital_out(9))
# socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31) + pow(2,32)*get_flag(32))
# socket_send_int(get_analog_out(0)*1000000)
# socket_send_int(get_analog_out(1)*1000000)
# socket_send_int(get_analog_in(0)*1000000)
# socket_send_int(get_analog_in(1)*1000000)
# socket_send_int(get_analog_in(2)*1000000)
# socket_send_int(get_analog_in(3)*1000000)
# exit_critical
# end
# send_io_state()
# while True:
# send_io_state()
# sync()
# end
# sync()
#end
......@@ -171,9 +175,9 @@ def driverProg():
socket_open(HOSTNAME, PORT)
send_out("hello")
thread_state = run statePublisherThread()
thread_wrench_state = run wrench_statePublisherThread()
thread_io_state = run io_statePublisherThread()
#thread_state = run statePublisherThread()
#thread_wrench_state = run wrench_statePublisherThread()
#thread_io_state = run io_statePublisherThread()
thread_servo = run servoThread()
# Servoes in a circle
......@@ -309,9 +313,9 @@ def driverProg():
end
#sleep(1)
kill thread_state
kill thread_wrench_state
kill thread_io_state
#kill thread_state
#kill thread_wrench_state
#kill thread_io_state
socket_send_int(MSG_QUIT)
end
driverProg()
ur_driver/src/ur_driver/driver.py
+ 59
− 169
View file @ d567e892
......@@ -36,9 +36,9 @@ prevent_programming = False
# q_actual = q_from_driver + offset
joint_offsets = {}
PORT=30002
RT_PORT=30003
DEFAULT_REVERSE_PORT = 50001
PORT=30002 # 10 Hz, RobotState
RT_PORT=30003 #125 Hz, RobotStateRT
DEFAULT_REVERSE_PORT = 50001 #125 Hz, custom data (from prog)
MSG_OUT = 1
MSG_QUIT = 2
......@@ -86,12 +86,11 @@ Q3 = [1.5,-0.2,-1.57,0,0,0]
connected_robot = None
connected_robot_lock = threading.Lock()
connected_robot_cond = threading.Condition(connected_robot_lock)
last_joint_states = None
last_joint_states_lock = threading.Lock()
pub_joint_states = rospy.Publisher('joint_states', JointState)
pub_joint_statesRT = rospy.Publisher('joint_statesRT', JointState)
pub_robot_stateRT = rospy.Publisher('robot_stateRT', RobotStateRTMsg)
pub_wrench = rospy.Publisher('wrench', WrenchStamped)
pub_io_states = rospy.Publisher('io_states', IOStates)
pub_io_states2 = rospy.Publisher('io_states2', IOStates)
#dump_state = open('dump_state', 'wb')
class EOF(Exception): pass
......@@ -192,22 +191,16 @@ class URConnection(object):
rospy.logfatal("Real robot is no longer enabled. Driver is fuxored")
time.sleep(2)
sys.exit(1)
# If the urscript program is not executing, then the driver
# needs to publish joint states using information from the
# robot state packet.
if self.robot_state != self.EXECUTING:
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "From binary state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, jd in enumerate(state.joint_data):
msg.position[i] = jd.q_actual + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = [jd.qd_actual for jd in state.joint_data]
msg.effort = [0]*6
pub_joint_states.publish(msg)
self.last_joint_states = msg
###
# IO-Support is EXPERIMENTAL
#
# Notes:
# - Where are the flags coming from? Do we need flags? No, as 'prog' does not use them and other scripts are not running!
# - analog_input2 and analog_input3 are within ToolData
# - What to do with the different analog_input/output_range/domain?
# - Shall we have appropriate ur_msgs definitions in order to reflect MasterboardData, ToolData,...?
###
# Use information from the robot state packet to publish IOStates
msg = IOStates()
......@@ -232,12 +225,6 @@ class URConnection(object):
#print "Publish IO-Data from robot state data"
pub_io_states.publish(msg)
# Notes:
# - analog_input2 and analog_input3 are within ToolData
# - Where are the flags coming from?
#
# - Shall we have appropriate ur_msgs definitions in order to reflect MasterboardData, ToolData,...?
# Updates the state machine that determines whether we can program the robot.
can_execute = (state.robot_mode_data.robot_mode in [RobotMode.READY, RobotMode.RUNNING])
......@@ -275,11 +262,17 @@ class URConnection(object):
if more:
self.__buf = self.__buf + more
# Attempts to extract a packet
packet_length, ptype = struct.unpack_from("!IB", self.__buf)
if len(self.__buf) >= packet_length:
packet, self.__buf = self.__buf[:packet_length], self.__buf[packet_length:]
self.__on_packet(packet)
#unpack_from requires a buffer of at least 48 bytes
while len(self.__buf) >= 48:
# Attempts to extract a packet
packet_length, ptype = struct.unpack_from("!IB", self.__buf)
#print("PacketLength: ", packet_length, "; BufferSize: ", len(self.__buf))
if len(self.__buf) >= packet_length:
packet, self.__buf = self.__buf[:packet_length], self.__buf[packet_length:]
self.__on_packet(packet)
else:
break
else:
self.__trigger_disconnected()
self.__keep_running = False
......@@ -289,15 +282,11 @@ class URConnection(object):
self.__keep_running = False
class URConnectionRT(object):
TIMEOUT = 1.0
DISCONNECTED = 0
CONNECTED = 1
READY_TO_PROGRAM = 2
EXECUTING = 3
def __init__(self, hostname, port):
self.__thread = None
......@@ -332,40 +321,14 @@ class URConnectionRT(object):
def __trigger_disconnected(self):
log("Robot disconnected")
self.robot_state = self.DISCONNECTED
def __trigger_ready_to_program(self):
rospy.loginfo("Robot ready to program")
def __trigger_halted(self):
log("Halted")
def __on_packet(self, buf):
now = rospy.get_rostime()
stateRT = RobotStateRT.unpack(buf)
self.last_stateRT = stateRT
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.stamp = now
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
......@@ -373,8 +336,20 @@ class URConnectionRT(object):
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)
#self.last_joint_states = msg
pub_joint_states.publish(msg)
with last_joint_states_lock:
last_joint_states = msg
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = now
wrench_msg.wrench.force.x = stateRT.tcp_force[0]
wrench_msg.wrench.force.y = stateRT.tcp_force[1]
wrench_msg.wrench.force.z = stateRT.tcp_force[2]
wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
pub_wrench.publish(wrench_msg)
def __run(self):
while self.__keep_running:
......@@ -383,13 +358,17 @@ class URConnectionRT(object):
more = self.__sock.recv(4096)
if more:
self.__buf = self.__buf + more
# Attempts to extract a packet
packet_length = struct.unpack_from("!i", self.__buf)[0]
print("PacketLength: ", packet_length, "; BufferSize: ", len(self.__buf))
if len(self.__buf) >= packet_length:
packet, self.__buf = self.__buf[:packet_length], self.__buf[packet_length:]
self.__on_packet(packet)
#unpack_from requires a buffer of at least 48 bytes
while len(self.__buf) >= 48:
# Attempts to extract a packet
packet_length = struct.unpack_from("!i", self.__buf)[0]
#print("PacketLength: ", packet_length, "; BufferSize: ", len(self.__buf))
if len(self.__buf) >= packet_length:
packet, self.__buf = self.__buf[:packet_length], self.__buf[packet_length:]
self.__on_packet(packet)
else:
break
else:
self.__trigger_disconnected()
self.__keep_running = False
......@@ -399,8 +378,6 @@ class URConnectionRT(object):
self.__keep_running = False
def setConnectedRobot(r):
global connected_robot, connected_robot_lock
with connected_robot_lock:
......@@ -430,15 +407,14 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
return more
else:
now = rospy.get_rostime()
if self.last_joint_states and \
self.last_joint_states.header.stamp < now - rospy.Duration(1.0):
if last_joint_states and \
last_joint_states.header.stamp < now - rospy.Duration(1.0):
rospy.logerr("Stopped hearing from robot (last heard %.3f sec ago). Disconnected" % \
(now - self.last_joint_states.header.stamp).to_sec())
raise EOF()
(now - last_joint_states.header.stamp).to_sec())
#raise EOF()
def handle(self):
self.socket_lock = threading.Lock()
self.last_joint_states = None
setConnectedRobot(self)
print "Handling a request"
try:
......@@ -463,93 +439,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
s, buf = buf[:i], buf[i+1:]
log("Out: %s" % s)
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3*(6*4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!%ii" % (3*6), buf, 0)
buf = buf[3*6*4:]
state = [s / MULT_jointstate for s in state_mult]
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.name = joint_names
msg.position = [0.0] * 6
for i, q_meas in enumerate(state[:6]):
msg.position[i] = q_meas + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = state[6:12]
msg.effort = state[12:18]
self.last_joint_states = msg
pub_joint_states.publish(msg)
elif mtype == MSG_WRENCH:
while len(buf) < (6*4):
buf = buf + self.recv_more()
state_mult = struct.unpack_from("!%ii" % (6), buf, 0)
buf = buf[6*4:]
state = [s / MULT_wrench for s in state_mult]
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = rospy.get_rostime()
wrench_msg.wrench.force.x = state[0]
wrench_msg.wrench.force.y = state[1]
wrench_msg.wrench.force.z = state[2]
wrench_msg.wrench.torque.x = state[3]
wrench_msg.wrench.torque.y = state[4]
wrench_msg.wrench.torque.z = state[5]
pub_wrench.publish(wrench_msg)
#gets all IO States and publishes them into a message
elif mtype == MSG_GET_IO:
#print "MSG_GET_IO"
#print type(buf)
#print len(buf)
#print "Buf:", [ord(b) for b in buf]
msg = IOStates()
#gets flag states (0-7)
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
for i in range(0, 8):
msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
#gets flag states (8-15)
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
for i in range(8, 16):
msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
#gets flag states (16-23)
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
for i in range(16, 24):
msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
#gets flag states (24-31)
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
for i in range(24, 32):
msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
#gets analog_in[2] state
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
inp /= MULT_analog
buf = buf[4:]
msg.analog_in_states.append(Analog(2, inp))
#gets analog_in[3] state
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
inp /= MULT_analog
buf = buf[4:]
msg.analog_in_states.append(Analog(3, inp))
pub_io_states2.publish(msg)
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF("Received quit")
......@@ -641,7 +531,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
# Returns the last JointState message sent out
def get_joint_states(self):
return self.last_joint_states
return last_joint_states
class TCPServer(SocketServer.TCPServer):
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment