Python PyPR2类代码示例

def timerActions( id ):
  

  	global a,b,x, y , last_x, tracking_data, csvFile, csvFileCounter,LostConnectionCounter,NumPeople, msgTryTimer, NEW_INTERACTION_INITIALISER
	
  	
  
  
 	if msgTryTimer == id :
 
    		
    		 b +=1   		    		
	    	 vel_x = x - last_x
    		 last_x = x
         	 if vel_x < -0.01:
    		    	PyPR2.moveArmWithJointPos(**initial_left)
			a+=1

		 tracking_data.append((x,y,x,y,time.time(),NumPeople,"empty",1,LostConnectionCounter))
		
		#b +=1
		#updateCsv()

		 with open(csvFile, "w") as output:
   		 		writer = csv.writer(output, lineterminator='\n')
    	   			writer.writerows(tracking_data)

def send_arm_joint_speed(q_dot, is_left_arm = False):
    if is_left_arm:
        g = gen_larm_joint_vel_dict(q_dot)
    else:
        g = gen_rarm_joint_vel_dict(q_dot)

    PyPR2.moveArmWithJointVelocity(**g)

def find_human():
	
	global HUMAN_DETECTION_COUNTER,revolve_counter
	while revolve_counter==1 and PyPR2.getHeadPos()[0]<1.5 and HUMAN_DETECTION_COUNTER==0:
			
	
				revolve_cw()
		

	
	while revolve_counter==-1 and PyPR2.getHeadPos()[0]>-1.5 and HUMAN_DETECTION_COUNTER==0:
	
				revolve_acw()

	while PyPR2.getHeadPos()[0] >1.2 and HUMAN_DETECTION_COUNTER==0:

				revolve_counter = -1
				revolve_acw()
				find_human()

		
	while PyPR2.getHeadPos()[0] <-1.2 and HUMAN_DETECTION_COUNTER==0:
		
				revolve_counter = 1
				revolve_cw()
				find_human()

	if HUMAN_DETECTION_COUNTER!=0:
				adjust_to_shooting(PyPR2.getHeadPos()[1])

def adjust_to_shooting():
	global last_proximity
	proximity = check_head_proximity()

	if proximity== True and last_proximity==False:
		PyPR2.moveBodyTo(0.0,0.0,(0.65)*PyPR2.getHeadPos()[0],1)
		#PyPR2.moveHeadTo(0.0,y)
	last_proximity = proximity

def deactivate_trajectory_input():
    global rt_cnt, rt_position, rt_velocity, rt_acceleration, rt_orientation
    rt_cnt = 0
    PyPR2.registerRawTrajectoryInput( None )
    assert PyPR2.useJointVelocityControl(False)
    rt_orientation   = None
    rt_position      = numpy.zeros(3)
    rt_velocity      = numpy.zeros(3)
    rt_acceleration  = numpy.zeros(3)

def twist_forearm(angle = 180.0, is_left_arm = True):
        if is_left_arm:
            joint_name = 'l_forearm_roll_joint'
        else:
            joint_name = 'r_forearm_roll_joint'
            
        jd   = PyPR2.getArmJointPositions(is_left_arm)
        jd[joint_name] = jd[joint_name] + angle*math.pi/180.0
        PyPR2.moveArmWithJointPos(**jd)    

def onHumanDetected(objtype, nameid, trackid, status):
	global csvCounter,csvFile,actionIdentifier,isNearest
	PyPR2.moveTorsoBy(0.1,2)
	PyPR2.say("This exit is closed,   please use the other exit")
	csvCounter += 1
	csvFile = "/home/demoshare/sid_stuff/aggressiveBehaviourExperiment/test"+str(csvCounter)+"_"+str(time.time())+".csv"

	actionIdentifier = "Human Detected"
	isNearest = False

def on_move_arm_failed( is_left_arm ):
    global larm_failed, rarm_failed
    if hasattr( PyPR2, 'onMoveArmPoseFailed' ) and hasattr( PyPR2.onMoveArmPoseFailed, '__call__' ):
      PyPR2.onMoveArmPoseFailed( is_left_arm )
    if is_left_arm:
        print "larm failed"
        larm_failed = True
    else:
        rarm_failed = True
        print "rarm failed"

def set_lg(x = 1):
    '''
    sets the position of left gripper from 0 to 8
    '''
    if x > 8:
        x = 8
    if x < 0:
        x = 0

    PyPR2.setGripperPosition(1, 0.01*x)

def set_rg(x = 1):
    '''
    sets the position of right gripper from 0 to 8
    '''
    if x > 8:
        x = 8
    if x < 0:
        x = 0

    PyPR2.setGripperPosition(2, 0.01*x)

def on_move_arm_finished( is_left_arm ):
    global larm_reached, rarm_reached
    if hasattr( PyPR2, 'onMoveArmPoseComplete' ) and hasattr( PyPR2.onMoveArmPoseComplete, '__call__' ):
      PyPR2.onMoveArmPoseComplete( is_left_arm )
    if is_left_arm:
        print "larm reached"
        larm_reached = True
    else:
        print "rarm reached"
        rarm_reached = True

def larm_elbow_position():
    '''
    returns the elbow position of the right arm comparable to function wrist_position() in the arm object
    '''    
    p0 = PyPR2.getRelativeTF('torso_lift_link' , 'l_shoulder_pan_link')['position']
    p  = PyPR2.getRelativeTF('torso_lift_link' , 'l_elbow_flex_link')['position']
    x = p[0] - p0[0]
    y = p[1] - p0[1]
    z = p[2] - p0[2]
    pos = numpy.array([x,y,z])
    return(pos)    

def restoreInitialState():
	global LostConnectionCounter, NumPeople, tracking_data, NEW_INTERACTION_INITIALISER,csvFile,csvFileCounter,last_x

	PyPR2.tuckBothArms()												#for the time being

	#NumPeople = 0
	tracking_data = ['focus_x','focus_y','avg_x','avg_y','time','speechCommand','actionCounter']
 	NEW_INTERACTION_INITIALISER = 0
 	csvFileCounter += 1
 	csvFile = "/home/demoshare/sid_stuff/Nov26TrackData/"+str(csvFileCounter)+".csv"										#change the approprite csvFile
	LostConnectionCounter =0 
	last_x = 0

 def onHumanDetected( self, objtype, trackid, nameid, status ):
   if status == NEW_OBJ or status == REC_OBJ:
     if nameid == 0:
       nameid = 3
     if nameid not in self.seen_people:
       self.seen_people[nameid] = KnownPerson( nameid )
       PyPR2.say( 'Hello {}'.format(self.seen_people[nameid].label) )
     elif self.seen_people[nameid].last_seen - time.time() > 15*60:
       PyPR2.say( 'Hello {}'.format(self.seen_people[nameid].label) )
   elif status == LOST_OBJ:
     if nameid in self.seen_people:
       self.seen_people[nameid].last_seen = time.time()

def activate_trajectory_input():
    '''
    To start receiving raw trajectory input, you should send a ROS service request call:
    rosservice call /rhyth_dmp/recall_dmp_traj <figure name> <amplitude> <frequency> <sampling frequency> <number of cycles>
    For example:
    rosservice call /rhyth_dmp/recall_dmp_traj fig8 0.05 0.2 20 3

    generates 3 cycles of figure eight with frequency 1.0
    '''
    global rt_cnt
    rt_cnt = 0
    PyPR2.registerRawTrajectoryInput( on_trajectory_received )
    assert PyPR2.useJointVelocityControl(True)