示例#1
0
    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()
示例#2
0
    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()