Python PyPR2.moveTorsoBy方法代码示例

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveTorsoBy [as 别名]
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

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveTorsoBy [as 别名]
def onHumanDetected(objtype, trackid, nameid, status):	
	global HUMAN_DETECTION_COUNTER,start_time,torso_position_counter,msgTryTimer
	#PyPR2.say("Target Detect ed")
	#PyPR2.moveTorsoBy(0.1,3)
	#PyPR2.moveBodyTo(0.1,0.0,0.0,4)
	if HUMAN_DETECTION_COUNTER==0:
	
		track_x.append(('x','time'))
	HUMAN_DETECTION_COUNTER+=1
	msgTryTimer=-1
	start_time = time.time()
	if torso_position_counter ==0:
		PyPR2.moveTorsoBy(0.1,3)
		torso_position_counter +=1

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveTorsoBy [as 别名]
def onHumanDetected(objtype, nameid, trackid, status):
	PyPR2.moveTorsoBy(0.1,2)
	PyPR2.say("This exit is closed")

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveTorsoBy [as 别名]
def timerActions( id ):
  global msgTryTimer,busy_moving,track_d,last_action_counter,d,start_time,x,y,elapsed_time,focus_obj,a


	
  
  
  
  if msgTryTimer == id :
    #PyPR2.removeTimer( msgTryTimer )
        #msgTryTimer += 1
    #while True:
       #time.sleep(1)
	
	if abs(a-focus_obj['est_pos'][0]) < 0.04:
		a = focus_obj['est_pos'][0]
		PyPR2.moveBodyTo(0.1,0.0,0.0,1)
	else:
		a = focus_obj['est_pos'][0]
        d = math.sqrt(math.pow(x,2)+math.pow(y,2))
        adjust_to_shooting()
	

	'''
	with open(csvfile, "w") as output:
   		 writer = csv.writer(output, lineterminator='\n')
    	         writer.writerows(track_d)
	'''
	
	#PyPR2.moveTorsoBy(0.03,5)
	#if abs(previous_pos - focus_obj['est_pos'][0])< 0.1:	
	#	PyPR2.moveHeadTo(0.2,1.0)
	#adjust_to_shooting()
        #if busymoving>0: busymoving-=1

	if d>=3.5:
		if last_action_counter <4 and last_action_counter!=0:
			#obj.larm_reference = True	
			#obj.arm_down()
			
			PyPR2.moveArmWithJointPos(**right_high_five)
			PyPR.moveArmWithJointPos(**shooting_back_down)
			last_action_counter=4
			#busymoving=3
		elif last_action_counter == 4:		
			PyPR2.moveTorsoBy(0.1,2)
			
			PyPR2.say("Move Back")
			#busymoving=5

			
		else:		
			PyPR2.moveArmWithJointPos(**left_shooting)
			last_action_counter=4
			#busymoving=5
	elif d<=3.5 and d>3:
		if last_action_counter <3 and last_action_counter!=0:
				PyPR2.moveArmWithJointPos(**shooting_down)
				#obj.larm_reference = False
				#obj.arm_right()
				#PyPR2.moveBodyTo(0.01,0.0,0.0,1)
				last_action_counter=3
				
		elif last_action_counter ==3:
				PyPR2.moveBodyTo(0.07,0.0,0.0,0.51)
				#busymoving=10

		elif last_action_counter ==0:
			PyPR2.moveArmWithJointPos(**left_shooting)
			PyPR2.moveArWithJointPos(**alt_right_shooting)
			#busymoving=10
			PyPR2.say("Move Back")
		else:
				PyPR2.moveArmWithJointPos(**alt_right_shooting)
				PyPR2.moveArmWithJointPos(**left_shooting)
				last_action_counter=3		

				

	elif d<=3 and d >2:
			
				if last_action_counter <2 and last_action_counter!=0:
					
					#obj.larm_reference = False
					#obj.arm_right()
					PyPR2.moveArmWithJointPos(**left_relax1)
					last_action_counter=2
				#elif last_action_counter==0:

				else:
					PyPR2.moveArmWithJointPos(**alt_right_shooting)
					PyPR2.moveArmWithJointPos(**left_shooting)
					PyPR2.closeGripper(2)
					#PyPR2.closeGripper(2)
					last_action_counter =2
			

				#previous_pos = focus_obj['est_pos'][0]
				
	                #movement_tracker.append(str(CONDITION_TAG)+":"+str(focus_obj['est_pos']))
#.........这里部分代码省略.........

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveTorsoBy [as 别名]
def onHumanTracking(tracking_objs):
 	global HUMAN_COUNTER, st_time,msgTryTimer,x,y,Numpeople,avg_y,avg_x,actionIdentifier,isNearest
 
	#focus_obj = tracking_objs[object_inde x]
	Numpeople = len(tracking_objs)
	avgPos(tracking_objs)
	
 	if len(tracking_objs) == 0:
 		#PyPR2.cancelMoveBodyAction()
		if HUMAN_COUNTER !=0:
			PyPR2.removeTimer(msgTryTimer)
			PyPR2.moveArmWithJointPos(**initial_left)
			PyPR2.moveArmWithJointPos(**initial_right)
			#msgTryTimer = -1
			#elapsed_time = time.time()-st_time

			HUMAN_COUNTER =0
			
		
			

 	elif len(tracking_objs) > 0:
		if HUMAN_COUNTER ==0:
			
			PyPR2.onTimer = timerActions
			msgTryTimer = PyPR2.addTimer(1,-1,0.5)
			#a +=1
			#st_time = time.time()
			#no_objTracker.append(elapsed_time)
			HUMAN_COUNTER= Numpeople
			
		object_index = closest_obj_index(tracking_objs)
		focus_obj = tracking_objs[object_index]


		x = focus_obj['est_pos'][0]
		y = focus_obj['est_pos'][1]
		d = math.sqrt((math.pow(x,2))+(math.pow(y,2)))

		if d<0.1:
			PyPR2.cancelMoveBodyAction()
			PyPR2.cancelMoveArmAction(True)
			PyPR2.cancelMoveArmAction(False)
			#c+=1
#		track_y.append(y)
	
		mid_x = focus_obj['bound'][0] + focus_obj['bound'][2] / 2
      			
		mid_y = focus_obj['bound'][1] + focus_obj['bound'][3] / 2
     				#print "track obj {} mid pt ({}.{})".format(focus_obj['track_id'],mid_x,mid_y)
      		ofs_x = mid_x - 320
      		ofs_y = mid_y - 240
      		chx = chy = 0.0
			
      		if math.fabs(ofs_x) > 10:
      			chx = -ofs_x * 90.0 / 640 * 0.01745329252	
				#head_yaw_list.append(chx)
				
      		if math.fabs(ofs_y) > 10:
 				chy = ofs_y * 90.0 / 640 * 0.01745329252
		PyPR2.updateHeadPos( chx, chy )
		

		if Numpeople ==1:

			if x>3.5:
				if isNearest == False:
					if y>0:
						PyPR2.moveArmWithJointPos(**initial_left)
						actionIdentifier= "initial_left"
					else:

						PyPR2.moveArmWithJointPos(**initial_right)
						actionIdentifier= "initial_right"
			elif 2.5<x<3.5:
				PyPR2.moveTorsoBy(0.1,2)
				if y>0:
					PyPR2.moveArmWithJointPos(**second_left)
					actionIdentifier = "second_left"
				else:
					PyPR2.moveArmWithJointPos(**second_right)
					actionIdentifier = "second_right"
			
			elif 1.5<x<2.5:
				PyPR2.moveTorsoBy(0.1,2)
				isNearest = True
				if y>0:
					PyPR2.moveArmWithJointPos(**third_left)
					actionIdentifier = "third_left"
				else:
					PyPR2.moveArmWithJointPos(**third_right)
					actionIdentifier = "third_right"
			else:
				isNearest = True

				PyPR2.moveArmWithJointPos(**full_stretch_left)
				PyPR2.moveArmWithJointPos(**full_stretch_right)
				actionIdentifier = "full_stretch"

		if 1<Numpeople<3:
#.........这里部分代码省略.........