diff --git a/ur_driver/prog b/ur_driver/prog
index 572a09eb4bab511685a662acc9b020311870903a..b06e81fee7f7c09e2916ff1e2a8bd3a126acce68 100644
--- a/ur_driver/prog
+++ b/ur_driver/prog
@@ -100,17 +100,11 @@ def driverProg():
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))
- #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(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
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index a0b1fc15e400ab07e38fdc27b593421fa160da93..352af1947a937e4ed906824de69d7b0bf6814493 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -58,6 +58,7 @@ MULT_jointstate = 10000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
MULT_analog = 1000000.0
+MULT_analog_robotstate = 0.1
#Bounds for SetPayload service
MIN_PAYLOAD = 0.0
@@ -86,6 +87,7 @@ connected_robot_cond = threading.Condition(connected_robot_lock)
pub_joint_states = rospy.Publisher('joint_states', JointState)
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
@@ -202,6 +204,36 @@ class URConnection(object):
msg.effort = [0]*6
pub_joint_states.publish(msg)
self.last_joint_states = msg
+
+ # Use information from the robot state packet to publish IOStates
+ msg = IOStates()
+ #gets digital in states
+ for i in range(0, 10):
+ msg.digital_in_states.append(DigitalIn(i, (state.masterboard_data.digital_input_bits & (1<<i))>>i))
+ #gets digital out states
+ for i in range(0, 10):
+ msg.digital_out_states.append(DigitalOut(i, (state.masterboard_data.digital_output_bits & (1<<i))>>i))
+ #gets analog_in[0] state
+ inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
+ msg.analog_in_states.append(Analog(0, inp))
+ #gets analog_in[1] state
+ inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
+ msg.analog_in_states.append(Analog(1, inp))
+ #gets analog_out[0] state
+ inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
+ msg.analog_out_states.append(Analog(0, inp))
+ #gets analog_out[1] state
+ inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
+ msg.analog_out_states.append(Analog(1, inp))
+ #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])
@@ -353,81 +385,39 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
#gets all IO States and publishes them into a message
elif mtype == MSG_GET_IO:
- #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()
- for i in range(0, 10):
- msg.digital_in_states.append(DigitalIn(i, (inp & (1<<i))>>i))
- #gets digital out states
+ #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, 10):
- msg.digital_out_states.append(DigitalOut(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
+ 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]
- inp /= MULT_analog
buf = buf[4:]
- msg.analog_out_states.append(Analog(0, inp))
- #gets analog_out[1] state
+ 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]
- inp /= MULT_analog
buf = buf[4:]
- msg.analog_out_states.append(Analog(1, inp))
- #gets analog_in[0] state
+ 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]
- inp /= MULT_analog
- buf = buf[4:]
- msg.analog_in_states.append(Analog(0, inp))
- #gets analog_in[1] 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(1, inp))
+ 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()
@@ -442,7 +432,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
inp /= MULT_analog
buf = buf[4:]
msg.analog_in_states.append(Analog(3, inp))
- pub_io_states.publish(msg)
+ pub_io_states2.publish(msg)
elif mtype == MSG_QUIT:
print "Quitting"