diff --git a/ur_driver/prog b/ur_driver/prog
index 2f0aff8c1b30d442bf8c30d64f237887e7efbf5a..de63229e2a067155451ec9b20215863494fbcc5c 100644
--- a/ur_driver/prog
+++ b/ur_driver/prog
@@ -20,6 +20,7 @@ def driverProg():
MULT_jointstate = 10000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
+ MULT_analog = 1000000.0
pi = 3.14159265359
def send_out(msg):
@@ -36,61 +37,7 @@ def driverProg():
socket_send_int(waypoint_id)
exit_critical
end
-
- thread statePublisherThread():
- def send_joint_state():
- 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]))
- 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(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]))
- 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(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)
- #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
+
SERVO_IDLE = 0
SERVO_RUNNING = 1
@@ -143,7 +90,6 @@ def driverProg():
socket_open(HOSTNAME, PORT)
send_out("hello")
- thread_state = run statePublisherThread()
thread_servo = run servoThread()
# Servoes in a circle
@@ -235,41 +181,41 @@ def driverProg():
elif mtype == MSG_SET_DIGITAL_OUT:
#send_out("Received Digital Out Signal")
# Reads the parameters
- params_mult = socket_read_binary_integer(1+6+1)
+ params_mult = socket_read_binary_integer(2)
if params_mult[0] == 0:
- send_out("Received no parameters for set_out message")
+ send_out("Received no parameters for set_digital_out message")
end
if params_mult[2] > 0:
- set_digital_out(params_mult[1], True)
+ set_digital_out(params_mult[1], True)
elif params_mult[2] == 0:
set_digital_out(params_mult[1], False)
end
elif mtype == MSG_SET_FLAG:
#send_out("Received Set Flag Signal")
# Reads the parameters
- params_mult = socket_read_binary_integer(1+6+1)
+ params_mult = socket_read_binary_integer(2)
if params_mult[0] == 0:
- send_out("Received no parameters for set_out message")
+ send_out("Received no parameters for set_flag message")
end
if params_mult[2] != 0:
- set_flag(params_mult[1], True)
+ set_flag(params_mult[1], True)
elif params_mult[2] == 0:
set_flag(params_mult[1], False)
end
elif mtype == MSG_SET_ANALOG_OUT:
#send_out("Received Analog Out Signal")
# Reads the parameters
- params_mult = socket_read_binary_integer(1+6+1)
+ params_mult = socket_read_binary_integer(2)
if params_mult[0] == 0:
- send_out("Received no parameters for set_out message")
+ send_out("Received no parameters for set_analog_out message")
end
- set_analog_out(params_mult[1], (params_mult[2] / 1000000))
+ set_analog_out(params_mult[1], (params_mult[2] / MULT_analog))
elif mtype == MSG_SET_TOOL_VOLTAGE:
#send_out("Received Tool Voltage Signal")
- # Reads the parameters
- params_mult = socket_read_binary_integer(1+6+1)
+ # Reads the parameters (also reads second dummy '0' integer)
+ params_mult = socket_read_binary_integer(2)
if params_mult[0] == 0:
- send_out("Received no parameters for set_out message")
+ send_out("Received no parameters for set_tool_voltage message")
end
set_tool_voltage(params_mult[1])
else:
@@ -278,8 +224,6 @@ def driverProg():
end
end
- #sleep(1)
- kill thread_state
socket_send_int(MSG_QUIT)
end
driverProg()
diff --git a/ur_driver/src/ur_driver/deserializeRT.py b/ur_driver/src/ur_driver/deserializeRT.py
new file mode 100644
index 0000000000000000000000000000000000000000..5fe97d779d67613fa47eb55d44668b228a66bb68
--- /dev/null
+++ b/ur_driver/src/ur_driver/deserializeRT.py
@@ -0,0 +1,118 @@
+import struct
+import copy
+
+class RobotStateRT(object):
+ __slots__ = ['time',
+ 'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target',
+ 'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values',
+ 'unused',
+ 'tcp_force', 'tool_vector', 'tcp_speed',
+ 'digital_input_bits', 'motor_temperatures', 'controller_timer',
+ 'test_value',
+ 'robot_mode', 'joint_modes']
+
+ @staticmethod
+ def unpack(buf):
+ offset = 0
+ message_size = struct.unpack_from("!i", buf, offset)[0]
+ offset+=4
+ if message_size != len(buf):
+ print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
+ raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
+
+ rs = RobotStateRT()
+ #time: 1x double (1x 8byte)
+ rs.time = struct.unpack_from("!d",buf, offset)[0]
+ offset+=8
+
+ #q_target: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.q_target = copy.deepcopy(all_values)
+
+ #qd_target: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.qd_target = copy.deepcopy(all_values)
+
+ #qdd_target: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.qdd_target = copy.deepcopy(all_values)
+
+ #i_target: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.i_target = copy.deepcopy(all_values)
+
+ #m_target: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.m_target = copy.deepcopy(all_values)
+
+ #q_actual: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.q_actual = copy.deepcopy(all_values)
+
+ #qd_actual: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.qd_actual = copy.deepcopy(all_values)
+
+ #i_actual: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.i_actual = copy.deepcopy(all_values)
+
+ #tool_acc_values: 3x double (3x 8byte)
+ all_values = list(struct.unpack_from("!ddd",buf, offset))
+ offset+=3*8
+ rs.tool_acc_values = copy.deepcopy(all_values)
+
+ #unused: 15x double (15x 8byte)
+ offset+=120
+ rs.unused = []
+
+ #tcp_force: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.tcp_force = copy.deepcopy(all_values)
+
+ #tool_vector: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.tool_vector = copy.deepcopy(all_values)
+
+ #tcp_speed: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.tcp_speed = copy.deepcopy(all_values)
+
+ #digital_input_bits: 1x double (1x 8byte) ?
+ rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
+ offset+=8
+
+ #motor_temperatures: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.motor_temperatures = copy.deepcopy(all_values)
+
+ #controller_timer: 1x double (1x 8byte)
+ rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
+ offset+=8
+
+ #test_value: 1x double (1x 8byte)
+ rs.test_value = struct.unpack_from("!d",buf, offset)[0]
+ offset+=8
+
+ #robot_mode: 1x double (1x 8byte)
+ rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
+ offset+=8
+
+ #joint_mode: 6x double (6x 8byte)
+ all_values = list(struct.unpack_from("!dddddd",buf, offset))
+ offset+=6*8
+ rs.joint_modes = copy.deepcopy(all_values)
+
+ return rs
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index 5588801bedec5633f15a433cf49280a4c06e885e..469e523d2220eb89424f1787ca9e377ff5cab29d 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -17,6 +17,7 @@ from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from geometry_msgs.msg import WrenchStamped
from ur_driver.deserialize import RobotState, RobotMode
+from ur_driver.deserializeRT import RobotStateRT
from ur_msgs.srv import SetPayload, SetIO
from ur_msgs.msg import *
@@ -35,8 +36,9 @@ prevent_programming = False
# q_actual = q_from_driver + offset
joint_offsets = {}
-PORT=30002
-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
@@ -57,6 +59,8 @@ MULT_wrench = 10000.0
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
@@ -82,6 +86,8 @@ 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_wrench = rospy.Publisher('wrench', WrenchStamped)
pub_io_states = rospy.Publisher('io_states', IOStates)
@@ -185,22 +191,40 @@ 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()
+ #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)
+
# 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])
@@ -238,11 +262,114 @@ 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
+
+ else:
+ self.__trigger_disconnected()
+ self.__keep_running = False
+
+
+class URConnectionRT(object):
+ TIMEOUT = 1.0
+
+ DISCONNECTED = 0
+ CONNECTED = 1
+
+ def __init__(self, hostname, port):
+ self.__thread = None
+ self.__sock = None
+ self.robot_state = self.DISCONNECTED
+ self.hostname = hostname
+ self.port = port
+ self.last_stateRT = None
+
+ def connect(self):
+ if self.__sock:
+ self.disconnect()
+ self.__buf = ""
+ self.robot_state = self.CONNECTED
+ self.__sock = socket.create_connection((self.hostname, self.port))
+ self.__keep_running = True
+ self.__thread = threading.Thread(name="URConnectionRT", target=self.__run)
+ self.__thread.daemon = True
+ self.__thread.start()
+
+ def disconnect(self):
+ if self.__thread:
+ self.__keep_running = False
+ self.__thread.join()
+ self.__thread = None
+ if self.__sock:
+ self.__sock.close()
+ self.__sock = None
+ self.last_state = None
+ self.robot_state = self.DISCONNECTED
+
+ def __trigger_disconnected(self):
+ log("Robot disconnected")
+ self.robot_state = self.DISCONNECTED
+
+ def __on_packet(self, buf):
+ global last_joint_states, last_joint_states_lock
+ now = rospy.get_rostime()
+ stateRT = RobotStateRT.unpack(buf)
+ self.last_stateRT = stateRT
+
+ msg = JointState()
+ msg.header.stamp = now
+ msg.header.frame_id = "From real-time state data"
+ msg.name = joint_names
+ msg.position = [0.0] * 6
+ for i, q in enumerate(stateRT.q_actual):
+ msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
+ msg.velocity = stateRT.qd_actual
+ msg.effort = [0]*6
+ 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:
+ r, _, _ = select.select([self.__sock], [], [], self.TIMEOUT)
+ if r:
+ more = self.__sock.recv(4096)
+ if more:
+ self.__buf = self.__buf + more
+
+ #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
@@ -272,6 +399,7 @@ def getConnectedRobot(wait=False, timeout=-1):
class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self):
+ global last_joint_states, last_joint_states_lock
while True:
r, _, _ = select.select([self.request], [], [], 0.2)
if r:
@@ -281,15 +409,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())
+ (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:
@@ -314,110 +441,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:
-
- #gets digital in states
- while len(buf) < 4:
- buf = buf + self.recv_more()
- 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
- 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
- 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 analog_out[0] state
- while len(buf) < 4:
- buf = buf + self.recv_more()
- inp = struct.unpack_from("!i", buf, 0)[0]
- inp /= 1000000.0
- buf = buf[4:]
- msg.analog_out_states.append(Analog(0, inp))
- #gets analog_out[1] state
- while len(buf) < 4:
- buf = buf + self.recv_more()
- inp = struct.unpack_from("!i", buf, 0)[0]
- inp /= 1000000.0
- buf = buf[4:]
- msg.analog_out_states.append(Analog(1, inp))
- #gets analog_in[0] state
- while len(buf) < 4:
- buf = buf + self.recv_more()
- inp = struct.unpack_from("!i", buf, 0)[0]
- inp /= 1000000.0
- 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 /= 1000000.0
- buf = buf[4:]
- msg.analog_in_states.append(Analog(1, inp))
- #gets analog_in[2] state
- while len(buf) < 4:
- buf = buf + self.recv_more()
- inp = struct.unpack_from("!i", buf, 0)[0]
- inp /= 1000000.0
- 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 /= 1000000.0
- buf = buf[4:]
- msg.analog_in_states.append(Analog(3, inp))
- pub_io_states.publish(msg)
-
+
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF("Received quit")
@@ -472,7 +496,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def set_analog_out(self, pinnum, value):
params = [MSG_SET_ANALOG_OUT] + \
[pinnum] + \
- [value * 1000000]
+ [value * MULT_analog]
buf = struct.pack("!%ii" % len(params), *params)
#print params
with self.socket_lock:
@@ -487,7 +511,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] + \
@@ -509,7 +533,8 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
# Returns the last JointState message sent out
def get_joint_states(self):
- return self.last_joint_states
+ global last_joint_states, last_joint_states_lock
+ return last_joint_states
class TCPServer(SocketServer.TCPServer):
@@ -925,6 +950,9 @@ def main():
connection.connect()
connection.send_reset_program()
+ connectionRT = URConnectionRT(robot_hostname, RT_PORT)
+ connectionRT.connect()
+
set_io_server()
service_provider = None
diff --git a/ur_driver/src/ur_driver/io_interface.py b/ur_driver/src/ur_driver/io_interface.py
index a7fa6d7ba65bdb9ac2509fdf4937a9076afbf0de..e78c0b4236aae3c773debffabf4db4ce96e2eb4c 100644
--- a/ur_driver/src/ur_driver/io_interface.py
+++ b/ur_driver/src/ur_driver/io_interface.py
@@ -10,49 +10,50 @@ FUN_SET_FLAG = 2
FUN_SET_ANALOG_OUT = 3
FUN_SET_TOOL_VOLTAGE = 4
-Flag_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Digital_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Digital_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Analog_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
-Analog_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+#Flag_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Digital_Out_States = [0,0,0,0,0,0,0,0,0,0] #8(controller)+2(tool)
+Digital_In_States = [0,0,0,0,0,0,0,0,0,0] #8(controller)+2(tool)
+Analog_Out_States = [0,0] #2(controller)
+Analog_In_States = [0,0] #2(controller)+0(tool)
ANALOG_TOLERANCE_VALUE = 0.01
-DELAY_TIME = 0.3
-i = 0
-set_io = rospy.ServiceProxy('set_io', SetIO)
def set_io_val(fun, pin, val):
- rospy.wait_for_service('set_io')
try:
set_io(fun, pin, val)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def set_tool_voltage(volts):
- set_io_val(FUN_SET_TOOL_VOLTAGE, volts, 0)
+ try:
+ set_io(FUN_SET_TOOL_VOLTAGE, volts, 0)
+ except rospy.ServiceException, e:
+ print "Service call failed: %s"%e
def set_digital_out(pin, val):
- set_io_val(FUN_SET_DIGITAL_OUT, pin, val)
- if Digital_Out_States[pin] != val:
- rospy.logwarn("DIGITAL OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
- set_digital_out(pin, val)
+ try:
+ set_io(FUN_SET_DIGITAL_OUT, pin, val)
+ except rospy.ServiceException, e:
+ print "Service call failed: %s"%e
def set_analog_out(pin, val):
- set_io_val(FUN_SET_ANALOG_OUT, pin, val)
- if abs(Analog_Out_States[pin] - val) > ANALOG_TOLERANCE_VALUE:
- rospy.logwarn("ANALOG OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
- set_digital_out(pin, val)
+ try:
+ set_io(FUN_SET_ANALOG_OUT, pin, val)
+ except rospy.ServiceException, e:
+ print "Service call failed: %s"%e
def set_flag(pin, val):
- set_io_val(FUN_SET_FLAG, pin, val)
- if Flag_States[pin] != val:
- rospy.logwarn("SETTING A FLAG IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
- set_digital_out(pin, val)
+ rospy.logerr("SETTING FLAGS IS NOT SUPPORTED!")
+ #try:
+ #set_io(FUN_SET_FLAG, pin, val)
+ #except rospy.ServiceException, e:
+ #print "Service call failed: %s"%e
def callback(data):
- for i in range(0,32):
- del Flag_States[i]
- Flag_States.insert(i, data.flag_states[i].state)
+ rospy.logerr("Flag_States are currently not supported")
+ #for i in range(0,32):
+ #del Flag_States[i]
+ #Flag_States.insert(i, data.flag_states[i].state)
for i in range(0,10):
del Digital_Out_States[i]
Digital_Out_States.insert(i, data.digital_out_states[i].state)
@@ -62,11 +63,17 @@ def callback(data):
for i in range(0,2):
del Analog_Out_States[i]
Analog_Out_States.insert(i, data.analog_out_states[i].state)
- for i in range(0,4):
+ rospy.logerr("ToolInput analog_in[2] and analog_in[3] currently not supported")
+ for i in range(0,2):
del Analog_In_States[i]
Analog_In_States.insert(i, data.analog_in_states[i].state)
def get_states():
rospy.init_node('UR_State_Getter')
rospy.Subscriber("io_states", IOStates, callback)
+
+def set_states():
+ rospy.wait_for_service('set_io')
+ global set_io
+ set_io = rospy.ServiceProxy('set_io', SetIO)
diff --git a/ur_driver/src/ur_driver/testRT_comm.py b/ur_driver/src/ur_driver/testRT_comm.py
new file mode 100755
index 0000000000000000000000000000000000000000..84c74142a553e2e52ba20584addd4881c593e1ed
--- /dev/null
+++ b/ur_driver/src/ur_driver/testRT_comm.py
@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('ur_driver')
+import time, sys, threading, math
+import copy
+import datetime
+import socket, select
+import struct
+import traceback, code
+import SocketServer
+
+import rospy
+
+from sensor_msgs.msg import JointState
+from ur_driver.deserializeRT import RobotStateRT
+from ur_msgs.msg import *
+
+
+joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+ 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+joint_offsets = {}
+
+
+def __on_packet(buf):
+ stateRT = RobotStateRT.unpack(buf)
+
+ 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.frame_id = "From real-time state data"
+ msg.name = joint_names
+ msg.position = [0.0] * 6
+ for i, q in enumerate(stateRT.q_actual):
+ 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)
+
+
+
+
+def main():
+ rospy.init_node('testRT_comm', disable_signals=True)
+
+ global pub_joint_statesRT
+ pub_joint_statesRT = rospy.Publisher('joint_statesRT', JointState)
+ global pub_robot_stateRT
+ pub_robot_stateRT = rospy.Publisher('robot_stateRT', RobotStateRTMsg)
+
+
+
+ robot_hostname = '192.168.0.42'
+ rt_port = 30003
+
+ rt_socket = socket.create_connection((robot_hostname, rt_port))
+ buf = ""
+
+ while not rospy.is_shutdown():
+ more = rt_socket.recv(4096)
+ if more:
+ buf = buf + more
+
+ # Attempts to extract a packet
+ packet_length = struct.unpack_from("!i", buf)[0]
+ print("PacketLength: ", packet_length, "; BufferSize: ", len(buf))
+ if len(buf) >= packet_length:
+ packet, buf = buf[:packet_length], buf[packet_length:]
+ __on_packet(packet)
+ else:
+ print("There is no more...")
+
+ rt_socket.close()
+
+
+
+
+if __name__ == '__main__': main()
diff --git a/ur_driver/src/ur_driver/test_comm.py b/ur_driver/src/ur_driver/test_comm.py
new file mode 100755
index 0000000000000000000000000000000000000000..29381354ccbf99050666cb8c68a982ce1625c847
--- /dev/null
+++ b/ur_driver/src/ur_driver/test_comm.py
@@ -0,0 +1,120 @@
+#!/usr/bin/env python
+import roslib; roslib.load_manifest('ur_driver')
+import time, sys, threading, math
+import copy
+import datetime
+import socket, select
+import struct
+import traceback, code
+import SocketServer
+
+import rospy
+
+from sensor_msgs.msg import JointState
+from ur_driver.deserialize import RobotState
+from ur_msgs.msg import *
+
+# renaming classes
+DigitalIn = Digital
+DigitalOut = Digital
+Flag = Digital
+
+MULT_analog = 1000000.0
+MULT_analog_robotstate = 0.1
+
+joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
+ 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+joint_offsets = {}
+
+
+def __on_packet(buf):
+ state = RobotState.unpack(buf)
+
+ mb_msg = MasterboardDataMsg()
+ mb_msg.digital_input_bits = state.masterboard_data.digital_input_bits
+ mb_msg.digital_output_bits = state.masterboard_data.digital_output_bits
+ mb_msg.analog_input_range0 = state.masterboard_data.analog_input_range0
+ mb_msg.analog_input_range1 = state.masterboard_data.analog_input_range1
+ mb_msg.analog_input0 = state.masterboard_data.analog_input0
+ mb_msg.analog_input1 = state.masterboard_data.analog_input1
+ mb_msg.analog_output_domain0 = state.masterboard_data.analog_output_domain0
+ mb_msg.analog_output_domain1 = state.masterboard_data.analog_output_domain1
+ mb_msg.analog_output0 = state.masterboard_data.analog_output0
+ mb_msg.analog_output1 = state.masterboard_data.analog_output1
+ mb_msg.masterboard_temperature = state.masterboard_data.masterboard_temperature
+ mb_msg.robot_voltage_48V = state.masterboard_data.robot_voltage_48V
+ mb_msg.robot_current = state.masterboard_data.robot_current
+ mb_msg.master_io_current = state.masterboard_data.master_io_current
+ mb_msg.master_safety_state = state.masterboard_data.master_safety_state
+ mb_msg.master_onoff_state = state.masterboard_data.master_onoff_state
+ pub_masterboard_state.publish(mb_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:
+ # - 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,...?
+
+
+
+
+
+def main():
+ rospy.init_node('test_comm', disable_signals=True)
+
+ global pub_masterboard_state
+ pub_masterboard_state = rospy.Publisher('masterboard_state', MasterboardDataMsg)
+ global pub_io_states
+ pub_io_states = rospy.Publisher('io_states', IOStates)
+
+
+ robot_hostname = '192.168.0.42'
+ port = 30002
+
+ test_socket = socket.create_connection((robot_hostname, port))
+ buf = ""
+
+ while not rospy.is_shutdown():
+ more = test_socket.recv(4096)
+ if more:
+ buf = buf + more
+
+ # Attempts to extract a packet
+ packet_length = struct.unpack_from("!i", buf)[0]
+ print("PacketLength: ", packet_length, "; BufferSize: ", len(buf))
+ if len(buf) >= packet_length:
+ packet, buf = buf[:packet_length], buf[packet_length:]
+ __on_packet(packet)
+ else:
+ print("There is no more...")
+
+ test_socket.close()
+
+
+
+
+if __name__ == '__main__': main()
diff --git a/ur_driver/test_io.py b/ur_driver/test_io.py
index 1d62d1703f8026e7d31ae11c20cda9352a45c358..7b3c2a82686bbea6d30a6b8b57becabaf7359fe7 100755
--- a/ur_driver/test_io.py
+++ b/ur_driver/test_io.py
@@ -3,23 +3,27 @@ import time
from ur_driver.io_interface import *
if __name__ == "__main__":
- print "toggling things"
+ print "testing io-interface"
get_states()
print "listener has been activated"
+ set_states()
+ print "service-server has been started"
while(True):
set_tool_voltage(12)
set_digital_out(0, True)
set_analog_out(0, 0.75)
- set_flag(0, True)
- print(Flag_States[0])
+ #print "Flags are currently not supported"
+ ##set_flag(0, True)
+ ##print(Flag_States[0])
print(Analog_Out_States[0])
print(Digital_Out_States[0])
time.sleep(1)
set_tool_voltage(24)
set_digital_out(0, False)
set_analog_out(0, 0.25)
- set_flag(0, False)
- print(Flag_States[0])
+ #print "Flags are currently not supported"
+ ##set_flag(0, False)
+ ##print(Flag_States[0])
print(Analog_Out_States[0])
print(Digital_Out_States[0])
time.sleep(1)
diff --git a/ur_msgs/CMakeLists.txt b/ur_msgs/CMakeLists.txt
index 1fc2dd9b027babcb8db1996ab0de0265aeb45ba5..20c5fef587a121af6f3dc04417662bbad5847b6a 100644
--- a/ur_msgs/CMakeLists.txt
+++ b/ur_msgs/CMakeLists.txt
@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
Analog.msg
Digital.msg
IOStates.msg
+ RobotStateRTMsg.msg
+ MasterboardDataMsg.msg
)
## Generate services in the 'srv' folder
diff --git a/ur_msgs/msg/MasterboardDataMsg.msg b/ur_msgs/msg/MasterboardDataMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..8ccdd6202e4dcd42c51d5efd5e45823961062736
--- /dev/null
+++ b/ur_msgs/msg/MasterboardDataMsg.msg
@@ -0,0 +1,27 @@
+# This data structure contains the MasterboardData structure
+# used by the Universal Robots controller
+#
+# MasterboardData is part of the data structure being send on the
+# secondary client communications interface
+#
+# This data structure is send at 10 Hz on TCP port 30002
+#
+# Dokumentation can be found on the Universal Robots Support Wiki
+# (http://wiki03.lynero.net/Technical/DataStreamFromURController?rev=8)
+
+int16 digital_input_bits
+int16 digital_output_bits
+int8 analog_input_range0
+int8 analog_input_range1
+float64 analog_input0
+float64 analog_input1
+int8 analog_output_domain0
+int8 analog_output_domain1
+float64 analog_output0
+float64 analog_output1
+float32 masterboard_temperature
+float32 robot_voltage_48V
+float32 robot_current
+float32 master_io_current
+uint8 master_safety_state
+uint8 master_onoff_state
diff --git a/ur_msgs/msg/RobotStateRTMsg.msg b/ur_msgs/msg/RobotStateRTMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..2741463973e0c1ed1d052f885e274fb912efc860
--- /dev/null
+++ b/ur_msgs/msg/RobotStateRTMsg.msg
@@ -0,0 +1,27 @@
+# Data structure for the realtime communications interface (aka Matlab interface)
+# used by the Universal Robots controller
+#
+# This data structure is send at 125 Hz on TCP port 30003
+#
+# Dokumentation can be found on the Universal Robots Support Wiki
+# (http://wiki03.lynero.net/Technical/RealTimeClientInterface?rev=9)
+
+float64 time
+float64[] q_target
+float64[] qd_target
+float64[] qdd_target
+float64[] i_target
+float64[] m_target
+float64[] q_actual
+float64[] qd_actual
+float64[] i_actual
+float64[] tool_acc_values
+float64[] tcp_force
+float64[] tool_vector
+float64[] tcp_speed
+float64 digital_input_bits
+float64[] motor_temperatures
+float64 controller_timer
+float64 test_value
+float64 robot_mode
+float64[] joint_modes