diff --git a/ur_driver/prog b/ur_driver/prog
index 5c3975210c97ce9f8f66351d4ca03196af52e6f2..653b1f514add2419de26771613f56d253b457980 100644
--- a/ur_driver/prog
+++ b/ur_driver/prog
@@ -43,7 +43,6 @@ def driverProg():
q = get_joint_positions()
qdot = get_joint_speeds()
tau = get_joint_torques()
- wrench = get_tcp_force()
enter_critical
socket_send_int(MSG_JOINT_STATES)
socket_send_int(floor(MULT_jointstate * q[0]))
@@ -64,6 +63,22 @@ def driverProg():
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]))
@@ -71,28 +86,55 @@ def driverProg():
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(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_out(0) * MULT_analog)
socket_send_int(get_analog_out(1) * MULT_analog)
socket_send_int(get_analog_in(0) * MULT_analog)
socket_send_int(get_analog_in(1) * MULT_analog)
socket_send_int(get_analog_in(2) * MULT_analog)
socket_send_int(get_analog_in(3) * MULT_analog)
- #socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
- #socket_open(HOSTNAME, 50002)
- send_joint_state()
+
+ #temporarily reduce publish rate for io_states
+ io_count = 0
+ io_slowdown_ratio = 10
+
+ send_io_state()
while True:
- send_joint_state()
+ if io_count != io_slowdown_ratio:
+ io_count=io_count+1
+ else:
+ send_io_state()
+ io_count=0
+ end
sync()
end
sync()
end
+
+
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
@@ -145,6 +187,8 @@ def driverProg():
send_out("hello")
thread_state = run statePublisherThread()
+ thread_wrench_state = run wrench_statePublisherThread()
+ thread_io_state = run io_statePublisherThread()
thread_servo = run servoThread()
# Servoes in a circle
@@ -281,6 +325,8 @@ def driverProg():
#sleep(1)
kill thread_state
+ kill thread_wrench_state
+ kill thread_io_state
socket_send_int(MSG_QUIT)
end
driverProg()
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index dcb28bdeda1537effc98d8e5ad311e5f1fe8b15f..e23199d19cf78d976e98dab1691bd6b94d915d0f 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -356,6 +356,10 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
#gets digital in states
while len(buf) < 4:
buf = buf + self.recv_more()
+ #print "MSG_GET_IO"
+ #print type(buf)
+ #print len(buf)
+ #print "Buf:", [ord(b) for b in buf]
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
msg = IOStates()
@@ -368,13 +372,34 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = buf[4:]
for i in range(0, 10):
msg.digital_out_states.append(DigitalOut(i, (inp & (1<<i))>>i))
- #gets flag states
- 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, 32):
- msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
+ ##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_out[0] state
while len(buf) < 4:
buf = buf + self.recv_more()
@@ -488,7 +513,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
#print params
with self.socket_lock:
self.request.send(buf)
- time.sleep(IO_SLEEP_TIME+.5)
+ time.sleep(IO_SLEEP_TIME)
def set_flag(self, pin, val):
params = [MSG_SET_FLAG] + \