def poll_gesture(self, **kwargs):
ges = self.blackboard["gesture"]
if not (ges.direction == 'o'):
r_cmd = robot_cmd()
todo = "left hand sweep"
r_cmd.cmd = todo
r_cmd.duration_10ms = 0
if ges.direction == 'l':
todo = "left hand strike 1" #"left hand sweep"
elif ges.direction == 'r':
todo = "right hand strike 1" #right hand sweep
elif ges.direction == 't':
todo = "right hand pickup"
elif ges.direction == 'b':
todo = "right hand throw"
#if ges.direction=='t':
#arm down
#arm up
#elif ges.direction=='b':
#arm up
#arm down
#elif ges.direction=='l':
#swipe left
#elif ges.direction=='r':
#swipe right
#later do action sequences
r_cmd.cmd = todo
self.blackboard["wake_up_time"] = 6000 #60 seconds
self.blackboard["pub_move"].publish(r_cmd)
self.blackboard["stop_counter"] = 400
self.blackboard["gesture"].direction = 'o'
yield True
def poll_gesture(self,**kwargs):
ges=self.blackboard["gesture"]
if not(ges.direction=='o'):
r_cmd=robot_cmd()
todo="left hand sweep"
r_cmd.cmd=todo
r_cmd.duration_10ms=0
if ges.direction=='l':
todo="left hand strike 1"#"left hand sweep"
elif ges.direction=='r':
todo="right hand strike 1"#right hand sweep
elif ges.direction=='t':
todo="right hand pickup"
elif ges.direction=='b':
todo="right hand throw"
#if ges.direction=='t':
#arm down
#arm up
#elif ges.direction=='b':
#arm up
#arm down
#elif ges.direction=='l':
#swipe left
#elif ges.direction=='r':
#swipe right
#later do action sequences
r_cmd.cmd=todo
self.blackboard["wake_up_time"]=6000 #60 seconds
self.blackboard["pub_move"].publish(r_cmd)
self.blackboard["stop_counter"]=400
self.blackboard["gesture"].direction='o'
yield True
def stt_cmd(self,stt):
func=stt_cmd_map[stt]
tp=func[0]
if not (tp==4):self.blackboard["to_track"]="null"
self.blackboard["stop_counter"]=func[1]
todo=func[2]
if tp==1:
#rospy.loginfo("move bot ="+todo)
self.blackboard["wake_up_time"]=6000 #60 seconds
r_cmd=robot_cmd()
r_cmd.cmd=todo
r_cmd.duration_10ms=0
self.blackboard["pub_move"].publish(r_cmd)
elif tp==2:
if todo=="t+" or todo=="t-":
ta=rospy.get_param("/robot/get_tilt")
tlt=0
if todo=="t+":tlt=ta+5
else:tlt=ta-5
v=Int32(tlt)
self.blackboard["pub_tilt"].publish(v)
elif todo=="p+" or todo=="p-":
pa=rospy.get_param("/robot/get_pan")
pn=0
if todo=="p+":pn=pa+5
else:pn=pa-5
v=Int32(pn)
self.blackboard["pub_pan"].publish(v)
else:
v=Int32(45)
self.blackboard["pub_pan"].publish(v)
self.blackboard["pub_tilt"].publish(v)
elif tp==3:
to_say="What"
if todo=="faces":
to_say="I saw "+str(board["num_faces"])+" faces"
elif todo=="sonar":
to_say=str(board["sonar_cm"])+" centimeters"
elif todo=="compass":
to_say=str(board["compass_deg"])+" degrees"
self.blackboard["pub_speak"].publish(to_say)
elif tp==4:
see_it=False
if todo=="friend":
self.blackboard["tracker_watch"]=900
self.blackboard["to_track"]="face"
if self.blackboard["face_memory"]>0:see_it=True
elif todo=="object":
self.blackboard["tracker_watch"]=900
self.blackboard["to_track"]="object"
#reset=String()
#reset.data="r"
#self.blackboard["pub_cmd"].publish(reset)#reset
to_say="I can't see proper"
if see_it:
self.blackboard["pub_bb"].publish(self.blackboard["Target"])
to_say="tracking "+todo
self.blackboard["pub_speak"].publish(to_say)
def stt_cmd(self, stt):
func = stt_cmd_map[stt]
tp = func[0]
if not (tp == 4): self.blackboard["to_track"] = "null"
self.blackboard["stop_counter"] = func[1]
todo = func[2]
if tp == 1:
#rospy.loginfo("move bot ="+todo)
self.blackboard["wake_up_time"] = 6000 #60 seconds
r_cmd = robot_cmd()
r_cmd.cmd = todo
r_cmd.duration_10ms = 0
self.blackboard["pub_move"].publish(r_cmd)
elif tp == 2:
if todo == "t+" or todo == "t-":
ta = rospy.get_param("/robot/get_tilt")
tlt = 0
if todo == "t+": tlt = ta + 5
else: tlt = ta - 5
v = Int32(tlt)
self.blackboard["pub_tilt"].publish(v)
elif todo == "p+" or todo == "p-":
pa = rospy.get_param("/robot/get_pan")
pn = 0
if todo == "p+": pn = pa + 5
else: pn = pa - 5
v = Int32(pn)
self.blackboard["pub_pan"].publish(v)
else:
v = Int32(45)
self.blackboard["pub_pan"].publish(v)
self.blackboard["pub_tilt"].publish(v)
elif tp == 3:
to_say = "What"
if todo == "faces":
to_say = "I saw " + str(board["num_faces"]) + " faces"
elif todo == "sonar":
to_say = str(board["sonar_cm"]) + " centimeters"
elif todo == "compass":
to_say = str(board["compass_deg"]) + " degrees"
self.blackboard["pub_speak"].publish(to_say)
elif tp == 4:
see_it = False
if todo == "friend":
self.blackboard["tracker_watch"] = 900
self.blackboard["to_track"] = "face"
if self.blackboard["face_memory"] > 0: see_it = True
elif todo == "object":
self.blackboard["tracker_watch"] = 900
self.blackboard["to_track"] = "object"
#reset=String()
#reset.data="r"
#self.blackboard["pub_cmd"].publish(reset)#reset
to_say = "I can't see proper"
if see_it:
self.blackboard["pub_bb"].publish(self.blackboard["Target"])
to_say = "tracking " + todo
self.blackboard["pub_speak"].publish(to_say)
def update_bb():
#read decrement counter
if board["stop_counter"] > 0:
board["stop_counter"] = board["stop_counter"] - 1
if board["stop_counter"] == 0:
#publish stop,
stp_cmd = robot_cmd()
stp_cmd.cmd = "stop"
stp_cmd.duration_10ms = 0
pub_move.publish(stp_cmd)
#reset num faces seen
#when 0 watchdog and low confidence discard bb, reset
cnt = board["tracker_watch"]
if cnt > 0:
cnt = cnt - 1
#else:
# board["to_track"]="null"
if board["to_track"] == "null": cnt = 0
board["tracker_watch"] = cnt
cnt = board["track_delay"]
if cnt > 0:
cnt = cnt - 1
board["track_delay"] = cnt
cnt = board["wake_up_time"]
if cnt > 0:
cnt = cnt - 1
else:
r_cmd = robot_cmd()
#r_cmd.cmd="wake up" #uses too much power causes reset.. to be fix in ckt
r_cmd.cmd = "stop"
r_cmd.duration_10ms = 0
board["pub_move"].publish(r_cmd)
cnt = 6000 #60 seconds
board["wake_up_time"] = cnt
cnt = board["face_memory"]
if cnt > 0:
cnt = cnt - 1
board["face_memory"] = cnt
cnt = board["objects_memory"]
if cnt > 0:
cnt = cnt - 1
board["objects_memory"] = cnt
def update_bb():
#read decrement counter
if board["stop_counter"]>0:
board["stop_counter"]=board["stop_counter"]-1
if board["stop_counter"]==0:
#publish stop,
stp_cmd=robot_cmd()
stp_cmd.cmd="stop"
stp_cmd.duration_10ms=0
pub_move.publish(stp_cmd)
#reset num faces seen
#when 0 watchdog and low confidence discard bb, reset
cnt=board["tracker_watch"]
if cnt>0:
cnt=cnt-1
#else:
# board["to_track"]="null"
if board["to_track"]=="null":cnt=0
board["tracker_watch"]=cnt
cnt=board["track_delay"]
if cnt>0:
cnt=cnt-1
board["track_delay"]=cnt
cnt=board["wake_up_time"]
if cnt>0:
cnt=cnt-1
else:
r_cmd=robot_cmd()
#r_cmd.cmd="wake up" #uses too much power causes reset.. to be fix in ckt
r_cmd.cmd="stop"
r_cmd.duration_10ms=0
board["pub_move"].publish(r_cmd)
cnt=6000 #60 seconds
board["wake_up_time"]=cnt
cnt=board["face_memory"]
if cnt>0:
cnt=cnt-1
board["face_memory"]=cnt
cnt=board["objects_memory"]
if cnt>0:
cnt=cnt-1
board["objects_memory"]=cnt
def update_bb():
#read decrement counter
if board["stop_counter"] > 0:
board["stop_counter"] = board["stop_counter"] - 1
if board["stop_counter"] == 0:
#publish stop,
stp_cmd = robot_cmd()
stp_cmd.cmd = "stop"
stp_cmd.duration_10ms = 0
pub_move.publish(stp_cmd)
def update_bb():
#read decrement counter
if board["stop_counter"]>0:
board["stop_counter"]=board["stop_counter"]-1
if board["stop_counter"]==0:
#publish stop,
stp_cmd=robot_cmd()
stp_cmd.cmd="stop"
stp_cmd.duration_10ms=0
pub_move.publish(stp_cmd)
def stt_cmd(self, stt):
func = stt_cmd_map[stt]
tp = func[0]
self.blackboard["stop_counter"] = func[1]
todo = func[2]
if tp == 1:
rospy.loginfo("move bot =" + todo)
r_cmd = robot_cmd()
r_cmd.cmd = todo
r_cmd.duration_10ms = 0
self.blackboard["pub_move"].publish(r_cmd)
elif tp == 2:
if todo == "t+" or todo == "t-":
ta = rospy.get_param("/robot/get_tilt")
tlt = 0
if todo == "t+": tlt = ta + 5
else: tlt = ta - 5
v = Int32(tlt)
self.blackboard["pub_tilt"].publish(v)
elif todo == "p+" or todo == "p-":
pa = rospy.get_param("/robot/get_pan")
pn = 0
if todo == "p+": pn = pa + 5
else: pn = pa - 5
v = Int32(pn)
self.blackboard["pub_pan"].publish(v)
else:
v = Int32(45)
self.blackboard["pub_pan"].publish(v)
self.blackboard["pub_tilt"].publish(v)
elif tp == 3:
to_say = "What"
if todo == "faces":
to_say = "I see " + str(board["num_faces"]) + " faces"
elif todo == "sonar":
to_say = str(board["sonar_cm"]) + " centimeters"
elif todo == "compass":
to_say = str(board["compass_deg"]) + " degrees"
self.blackboard["pub_speak"].publish(to_say)
def stt_cmd(self,stt):
func=stt_cmd_map[stt]
tp=func[0]
self.blackboard["stop_counter"]=func[1]
todo=func[2]
if tp==1:
rospy.loginfo("move bot ="+todo)
r_cmd=robot_cmd()
r_cmd.cmd=todo
r_cmd.duration_10ms=0
self.blackboard["pub_move"].publish(r_cmd)
elif tp==2:
if todo=="t+" or todo=="t-":
ta=rospy.get_param("/robot/get_tilt")
tlt=0
if todo=="t+":tlt=ta+5
else:tlt=ta-5
v=Int32(tlt)
self.blackboard["pub_tilt"].publish(v)
elif todo=="p+" or todo=="p-":
pa=rospy.get_param("/robot/get_pan")
pn=0
if todo=="p+":pn=pa+5
else:pn=pa-5
v=Int32(pn)
self.blackboard["pub_pan"].publish(v)
else:
v=Int32(45)
self.blackboard["pub_pan"].publish(v)
self.blackboard["pub_tilt"].publish(v)
elif tp==3:
to_say="What"
if todo=="faces":
to_say="I see "+str(board["num_faces"])+" faces"
elif todo=="sonar":
to_say=str(board["sonar_cm"])+" centimeters"
elif todo=="compass":
to_say=str(board["compass_deg"])+" degrees"
self.blackboard["pub_speak"].publish(to_say)
def searchTurnLeft():
init_cam()
cmd = robot_cmd('turn left', 200)
robot_cmd_pub.publish(cmd)
def move_forward(er):
factor = tilt_max - tilt_min
#cmd=robot_cmd('walk forward',(float(er)/factor)*199+1)
#cmd=robot_cmd('walk forward',clamp(10,(float(er)/float(factor))*199+1,200))
cmd = robot_cmd('walk forward', clamp(10, 200, 200))
robot_cmd_pub.publish(cmd)
def correctRight(er):
#map er to time 1 to 200
factor = 50.0
cmd = robot_cmd('turn right', clamp(10, (float(er) / factor) * 199 + 1,
200))
robot_cmd_pub.publish(cmd)
def searchTurnLeft():
init_cam()
cmd=robot_cmd('turn left',200)
robot_cmd_pub.publish(cmd)
def move_forward(er):
factor=tilt_max-tilt_min
#cmd=robot_cmd('walk forward',(float(er)/factor)*199+1)
#cmd=robot_cmd('walk forward',clamp(10,(float(er)/float(factor))*199+1,200))
cmd=robot_cmd('walk forward',clamp(10,200,200))
robot_cmd_pub.publish(cmd)
def correctRight(er):
#map er to time 1 to 200
factor=50.0
cmd=robot_cmd('turn right',clamp(10,(float(er)/factor)*199+1,200))
robot_cmd_pub.publish(cmd)
