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

send io-states using robot state data - proof-of-concet

parent c54fd389
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ur_driver/prog
+ 5
− 11
View file @ 7a90b55d
......@@ -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
......
ur_driver/src/ur_driver/driver.py
+ 45
− 55
View file @ 7a90b55d
......@@ -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"
......
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