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 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
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
def onHumanDetected(objtype, nameid, trackid, status):
PyPR2.moveTorsoBy(0.1,2)
PyPR2.say("This exit is closed")
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']))
elif d <2 and d>1 :
if last_action_counter ==1:
PyPR2.say("Move")
last_action_counter=1
#busymoving=10
elif last_action_counter >=2:
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.moveArmWithJointPos(**alt_right_shooting)
else :
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPr2.moveArmWithJointPos(**left_shooting)
def onHumanDetected(objtype, nameid, trackid, status):
PyPR2.moveTorsoBy(0.1, 2)
PyPR2.say("This exit is closed")
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:
if x>3.5:
if isNearest ==False:
PyPR2.moveArmWithJointPos(**initial_left)
PyPR2.moveArmWithJointPos(**initial_right)
actionIdentifier = "Both_initial"
elif 2.5<x<3.5:
PyPR2.moveArmWithJointPos(**second_left)
PyPR2.moveArmWithJointPos(**second_right)
PyPR2.moveTorsoBy(0.1,2)
actionIdentifier = "Multiple_Second"
elif 1.5<x<2.5:
isNearest = True
PyPR2.moveArmWithJointPos(**third_right)
PyPR2.moveArmWithJointPos(**third_left)
PyPR2.moveTorsoBy(0.1,2)
actionIdentifier = "Multiple_third"
else:
isNearest = True
PyPR2.moveArmWithJointPos(**full_stretch_left)
PyPR2.moveArmWithJointPos(**full_stretch_right)
actionIdentifier = "Full_Multiple"
elif Numpeople > 3:
isNearest = True
PyPR2.moveArmWithJointPos(**full_stretch_left)
PyPR2.moveArmWithJointPos(**full_stretch_right)
actionIdentifier = "TooManyPeopleBehaviour"
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']))
elif d < 2 and d > 1:
if last_action_counter == 1:
PyPR2.say("Move")
last_action_counter = 1
#busymoving=10
elif last_action_counter >= 2:
PyPR2.moveArmWithJointPos(**left_shooting)
PyPR2.moveArmWithJointPos(**alt_right_shooting)
else:
PyPR2.moveArmWithJointPos(**alt_right_shooting)
PyPr2.moveArmWithJointPos(**left_shooting)