Exemple #1
0
    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)
Exemple #4
0
    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)
Exemple #5
0
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)
Exemple #11
0
def searchTurnLeft():
    init_cam()
    cmd = robot_cmd('turn left', 200)
    robot_cmd_pub.publish(cmd)
Exemple #12
0
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)
Exemple #13
0
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)
Exemple #14
0
def searchTurnLeft():
    init_cam()
    cmd=robot_cmd('turn left',200)
    robot_cmd_pub.publish(cmd)
Exemple #15
0
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)
Exemple #16
0
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)