def update_loop(self):
control_rate = rospy.Rate(100)
bimanual_chan = ach.Channel("bimanual_chan")
bimanual_chan.flush()
bimanual_msg = bytearray(100000)
while not rospy.is_shutdown():
[status,framesize] = bimanual_chan.get( bimanual_msg, wait=False, last=False )
if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME :
ml = JointTrajectory()
mr = JointTrajectory()
print "About to read msg"
(bl, mode, br) = interloper.readGatekeeperMsg(bimanual_msg)
print "Read msgs and mode: ", mode
if len(bl) > 0 :
ml.deserialize(bl)
print "L points: ", len(ml.points)
if len(br) > 0 :
mr.deserialize(br)
print "R points: ", len(mr.points)
print "Deserialized msgs"
control_rate.sleep()
def update_loop(self):
control_rate = rospy.Rate(100)
bimanual_chan = ach.Channel("bimanual_chan")
bimanual_hand_chan = ach.Channel("bimanual_hand_chan")
bimanual_chan.flush()
bimanual_hand_chan.flush()
bimanual_msg = bytearray(100000)
bimanual_hand_msg = bytearray(100000)
jt_msg_left = JointTrajectory()
jt_msg_right = JointTrajectory()
while not rospy.is_shutdown():
# Grab arm states and send them to robot
pl = [ self._arms['left']._joint_angle[i] for i in self._arms['left']._joint_names['left'] ]
vl = [ self._arms['left']._joint_velocity[i] for i in self._arms['left']._joint_names['left'] ]
pr = [ self._arms['right']._joint_angle[i] for i in self._arms['right']._joint_names['right'] ]
vr = [ self._arms['right']._joint_velocity[i] for i in self._arms['right']._joint_names['right'] ]
# Grab hand state and send it to robot
hl = self._hands['left'].position()*(1.0/4000.0) # Mapping 0-100 to 0-0.025cm
hr = 0
interloper.sendArmStates( np.asarray(pl), np.asarray(vl), 'state-left' )
interloper.sendArmStates( np.asarray(pr), np.asarray(vr), 'state-right' )
interloper.sendHandStates( hl, 0, 'gripperstate-left' )
interloper.sendHandStates( hr, 0, 'gripperstate-right' )
# Check if bimanual information was received
[status,framesize] = bimanual_chan.get( bimanual_msg, wait=False, last=True )
if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME :
(jt_msg_left_ser, mode, jt_msg_right_ser) = interloper.readGatekeeperMsg(bimanual_msg)
print "Mode python: ", mode
print "Size left: ", len(jt_msg_left_ser), " and right: ", len(jt_msg_right_ser)
if len(jt_msg_left_ser) > 0 :
print "Follow trajectory left"
jt_msg_left.deserialize( jt_msg_left_ser )
self.followTrajectory( jt_msg_left, 'left' )
if len(jt_msg_right_ser) > 0 :
print "Follow trajectory right"
jt_msg_right.deserialize( jt_msg_right_ser )
self.followTrajectory( jt_msg_right, 'right' )
# Clean after yourself
bimanual_chan.flush()
# Check if bimanual_hand info has been received
[status,framesize] = bimanual_hand_chan.get( bimanual_hand_msg, wait=False, last=True )
if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME :
(hand_pos_left, mode, hand_pos_right) = interloper.readGatekeeperHandMsg(bimanual_hand_msg)
print "Mode python: ", mode
if mode == 0 :
print "Moving hand left ", hand_pos_left, " and right: ", hand_pos_right
self._hands['left'].command_position( hand_pos_left, False, 5.0)
rospy.sleep(2.0)
interloper.sendGatekeeperMsg( "left_gripper", 1, "gatekeeper_msg_chan" )
if mode == 1 :
print "Right hand not ON"
# Clean after yourself
bimanual_hand_chan.flush()
# Send these as sns_msg_state
control_rate.sleep()
