From d567e892dedc90f536c6634b0a54762a4ff29578 Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Tue, 30 Sep 2014 12:26:29 +0000
Subject: [PATCH] main commit: use ur socket protocols to reduce load on prog
---
ur_driver/prog | 172 +++++++++++-----------
ur_driver/src/ur_driver/driver.py | 228 ++++++++----------------------
2 files changed, 147 insertions(+), 253 deletions(-)
diff --git a/ur_driver/prog b/ur_driver/prog
index b06e81f..4104e0f 100644
--- a/ur_driver/prog
+++ b/ur_driver/prog
@@ -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()
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index 2410798..c2a594e 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -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):
--
GitLab