Esempio n. 1
0
def createVFH(verbose=False):
    return VFH(robot,
               laserFOV=fov,
               blockingDistance=0.,
               safetyDistance=0.10,
               maxRange=1.5,
               turnRadius=0.5,
               binSize=binSize,
               verbose=verbose)
Esempio n. 2
0
  def goToVfhDigit( self, digit, timeout, info = None ):
    TOLERATED_MISS = 0.2 # [m]
    ANGULAR_THRESHOLD = math.radians(20) # [rad]
    ANGULAR_SPEED = math.radians(60) # [rad/s]

    ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
    RETREAT = 0.2 # A retreated distance after endurance times out. [m]

    pathFinder = LaserPath( verbose=False )
    self.robot.addExtension( pathFinder.updateExtension, "PATH" )

    turnRadius = None #1.2
#    vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
    vfh = VFH(self.robot, blockingDistance = 0.2, safetyDistance = 0.3, maxRange = 1.0, turnRadius = None, verbose = False)
    self.robot.addExtension(vfh.updateExtension, "VHF")
    # Navigate roughly in the suggested direction, avoiding obstacles.
    prevDir = 0.0

    waypoint = self.cam2waypoint( self.robot.localisation.pose(), info )
    goalDir = self.waypoint2dir( self.robot.localisation.pose(), waypoint )
    print "GOAL DIR", goalDir

    oldCam = self.robot.cameraData
    countImages = 0
    countVerified = 0

    startTime = self.robot.time
    happyTime = self.robot.time
    while self.robot.time < startTime + timeout:
      if oldCam != self.robot.cameraData:
        oldCam = self.robot.cameraData
        countImages += 1
        if self.robot.cameraData and self.robot.cameraData[0]:
          info = None
          for d in parseCameraData( self.robot.cameraData ):
            # TODO switch to goToDigit for large enough number
            if d[0] == digit:
              info = d
          if info != None:
            countVerified += 1
            waypoint = self.cam2waypoint( self.robot.localisation.pose(), info )
            goalDir = self.waypoint2dir( self.robot.localisation.pose(), waypoint )
            print "GOAL DIR", goalDir, oldCam, info, digit

      dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
      if dir is None or not pathFinder.isTraversable(dir):
        strategy = zeroCmd()
        prevDir = 0.0
        self.robot.toDisplay = 'HH'
      else:
        pose = self.robot.localisation.pose()
        goal = combinedPose( (pose[0], pose[1], pose[2]+dir), (1.0, 0, 0) )
        strategy = self.driver.goToG( goal , TOLERATED_MISS, angleThreshold = ANGULAR_THRESHOLD,  angularSpeed = ANGULAR_SPEED)
        prevDir = dir
        self.robot.toDisplay = 'OK'
        happyTime = self.robot.time

      if self.robot.time - happyTime > ENDURANCE:
        break # does not have sense to wait - try to turn somewhere else ...
        # We are stuck for too long. Let's try to retreat a bit:
        #    1) to show we are alive,
        #    2) to hope the robot gets out of the blocked state.
        # Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
        pose = self.robot.localisation.pose()
        goal = combinedPose( pose, (-(RETREAT + TOLERATED_MISS), 0, 0) )
        strategy = self.driver.goToG( goal , TOLERATED_MISS, backward = True, angleThreshold = ANGULAR_THRESHOLD,  angularSpeed = ANGULAR_SPEED)
        self.robot.toDisplay = '\/'
        for (speed, angularSpeed) in strategy:
          self.robot.setSpeedPxPa( speed, angularSpeed )
          self.robot.update()
        happyTime = self.robot.time
        continue # TODO maybe replace by return -> give up

      for (speed, angularSpeed) in strategy:
        self.robot.setSpeedPxPa( speed, angularSpeed )
        self.robot.update()
        pose = self.robot.localisation.pose()
#        isThere = math.hypot(pose[0] - lastX, pose[1] - lastY) <= TOLERATED_MISS
#        if isThere or pathFinder.newData:
        if pathFinder.newData:
          pathFinder.newData = False
          break

    self.robot.removeExtension( "VHF" )
    self.robot.removeExtension( "PATH" )
Esempio n. 3
0
    def goToVfhDigit(self, digit, timeout, info=None):
        TOLERATED_MISS = 0.2  # [m]
        ANGULAR_THRESHOLD = math.radians(20)  # [rad]
        ANGULAR_SPEED = math.radians(60)  # [rad/s]

        ENDURANCE = 3  # Amount of time the robot remains blocked before timing out and moving backwards. [s]
        RETREAT = 0.2  # A retreated distance after endurance times out. [m]

        pathFinder = LaserPath(verbose=False)
        self.robot.addExtension(pathFinder.updateExtension, "PATH")

        turnRadius = None  #1.2
        #    vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
        vfh = VFH(self.robot,
                  blockingDistance=0.2,
                  safetyDistance=0.3,
                  maxRange=1.0,
                  turnRadius=None,
                  verbose=False)
        self.robot.addExtension(vfh.updateExtension, "VHF")
        # Navigate roughly in the suggested direction, avoiding obstacles.
        prevDir = 0.0

        waypoint = self.cam2waypoint(self.robot.localisation.pose(), info)
        goalDir = self.waypoint2dir(self.robot.localisation.pose(), waypoint)
        print "GOAL DIR", goalDir

        oldCam = self.robot.cameraData
        countImages = 0
        countVerified = 0

        startTime = self.robot.time
        happyTime = self.robot.time
        while self.robot.time < startTime + timeout:
            if oldCam != self.robot.cameraData:
                oldCam = self.robot.cameraData
                countImages += 1
                if self.robot.cameraData and self.robot.cameraData[0]:
                    info = None
                    for d in parseCameraData(self.robot.cameraData):
                        # TODO switch to goToDigit for large enough number
                        if d[0] == digit:
                            info = d
                    if info != None:
                        countVerified += 1
                        waypoint = self.cam2waypoint(
                            self.robot.localisation.pose(), info)
                        goalDir = self.waypoint2dir(
                            self.robot.localisation.pose(), waypoint)
                        print "GOAL DIR", goalDir, oldCam, info, digit

            dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
            if dir is None or not pathFinder.isTraversable(dir):
                strategy = zeroCmd()
                prevDir = 0.0
                self.robot.toDisplay = 'HH'
            else:
                pose = self.robot.localisation.pose()
                goal = combinedPose((pose[0], pose[1], pose[2] + dir),
                                    (1.0, 0, 0))
                strategy = self.driver.goToG(goal,
                                             TOLERATED_MISS,
                                             angleThreshold=ANGULAR_THRESHOLD,
                                             angularSpeed=ANGULAR_SPEED)
                prevDir = dir
                self.robot.toDisplay = 'OK'
                happyTime = self.robot.time

            if self.robot.time - happyTime > ENDURANCE:
                break  # does not have sense to wait - try to turn somewhere else ...
                # We are stuck for too long. Let's try to retreat a bit:
                #    1) to show we are alive,
                #    2) to hope the robot gets out of the blocked state.
                # Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
                pose = self.robot.localisation.pose()
                goal = combinedPose(pose, (-(RETREAT + TOLERATED_MISS), 0, 0))
                strategy = self.driver.goToG(goal,
                                             TOLERATED_MISS,
                                             backward=True,
                                             angleThreshold=ANGULAR_THRESHOLD,
                                             angularSpeed=ANGULAR_SPEED)
                self.robot.toDisplay = '\/'
                for (speed, angularSpeed) in strategy:
                    self.robot.setSpeedPxPa(speed, angularSpeed)
                    self.robot.update()
                happyTime = self.robot.time
                continue  # TODO maybe replace by return -> give up

            for (speed, angularSpeed) in strategy:
                self.robot.setSpeedPxPa(speed, angularSpeed)
                self.robot.update()
                pose = self.robot.localisation.pose()
                #        isThere = math.hypot(pose[0] - lastX, pose[1] - lastY) <= TOLERATED_MISS
                #        if isThere or pathFinder.newData:
                if pathFinder.newData:
                    pathFinder.newData = False
                    break

        self.robot.removeExtension("VHF")
        self.robot.removeExtension("PATH")
Esempio n. 4
0
  def goVfh( self, timeout, maxDist=None ):
    print "goVfh", maxDist
    TOLERATED_MISS = 0.2 # [m]
    ANGULAR_THRESHOLD = math.radians(20) # [rad]
    ANGULAR_SPEED = math.radians(60) # [rad/s]

    ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
    RETREAT = 0.2 # A retreated distance after endurance times out. [m]

    pathFinder = LaserPath( verbose=False )
    self.robot.addExtension( pathFinder.updateExtension, "PATH" )

    turnRadius = None #1.2
#    vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
    vfh = VFH(self.robot, blockingDistance = 0.2, safetyDistance = 0.3, maxRange = 1.0, turnRadius = None, verbose = False)
    self.robot.addExtension(vfh.updateExtension, "VHF")
    # Navigate roughly in the suggested direction, avoiding obstacles.
    prevDir = 0.0
    goalDir = 0.0

    startTime = self.robot.time
    happyTime = self.robot.time
    distTraveled = 0.0
    while self.robot.time < startTime + timeout:
      if maxDist is not None and distTraveled > maxDist:
        print "goVfh - maxDist reached"
        break
      dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
      if dir is None or not pathFinder.isTraversable(dir):
        strategy = zeroCmd()
        prevDir = 0.0
        self.robot.toDisplay = 'HH'
      else:
        pose = self.robot.localisation.pose()
        goal = combinedPose( (pose[0], pose[1], pose[2]+dir), (1.0, 0, 0) )
        strategy = self.driver.goToG( goal , TOLERATED_MISS, angleThreshold = ANGULAR_THRESHOLD,  angularSpeed = ANGULAR_SPEED)
        prevDir = dir
        self.robot.toDisplay = 'OK'
        happyTime = self.robot.time

      if self.robot.time - happyTime > ENDURANCE:
        print "ENDURANCE!"
        break # does not have sense to wait - try to turn somewhere else ...
        # We are stuck for too long. Let's try to retreat a bit:
        #    1) to show we are alive,
        #    2) to hope the robot gets out of the blocked state.
        # Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
        pose = self.robot.localisation.pose()
        goal = combinedPose( pose, (-(RETREAT + TOLERATED_MISS), 0, 0) )
        strategy = self.driver.goToG( goal , TOLERATED_MISS, backward = True, angleThreshold = ANGULAR_THRESHOLD,  angularSpeed = ANGULAR_SPEED)
        self.robot.toDisplay = '\/'
        for (speed, angularSpeed) in strategy:
          self.robot.setSpeedPxPa( speed, angularSpeed )
          self.robot.update()
          distTraveled += self.robot.lastDistStep
        happyTime = self.robot.time
        continue # TODO maybe replace by return -> give up

      for (speed, angularSpeed) in strategy:
        self.robot.setSpeedPxPa( speed, angularSpeed )
        self.robot.update()
        distTraveled += self.robot.lastDistStep
        pose = self.robot.localisation.pose()
        if pathFinder.newData:
          pathFinder.newData = False
          break

    self.robot.removeExtension( "VHF" )
    self.robot.removeExtension( "PATH" )
    print "... traveled", distTraveled, "time", self.robot.time - startTime
Esempio n. 5
0
    def goVfh(self, timeout, maxDist=None):
        print "goVfh", maxDist
        TOLERATED_MISS = 0.2  # [m]
        ANGULAR_THRESHOLD = math.radians(20)  # [rad]
        ANGULAR_SPEED = math.radians(60)  # [rad/s]

        ENDURANCE = 3  # Amount of time the robot remains blocked before timing out and moving backwards. [s]
        RETREAT = 0.2  # A retreated distance after endurance times out. [m]

        pathFinder = LaserPath(verbose=False)
        self.robot.addExtension(pathFinder.updateExtension, "PATH")

        turnRadius = None  #1.2
        #    vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
        vfh = VFH(self.robot,
                  blockingDistance=0.2,
                  safetyDistance=0.3,
                  maxRange=1.0,
                  turnRadius=None,
                  verbose=False)
        self.robot.addExtension(vfh.updateExtension, "VHF")
        # Navigate roughly in the suggested direction, avoiding obstacles.
        prevDir = 0.0
        goalDir = 0.0

        startTime = self.robot.time
        happyTime = self.robot.time
        distTraveled = 0.0
        while self.robot.time < startTime + timeout:
            if maxDist is not None and distTraveled > maxDist:
                print "goVfh - maxDist reached"
                break
            dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
            if dir is None or not pathFinder.isTraversable(dir):
                strategy = zeroCmd()
                prevDir = 0.0
                self.robot.toDisplay = 'HH'
            else:
                pose = self.robot.localisation.pose()
                goal = combinedPose((pose[0], pose[1], pose[2] + dir),
                                    (1.0, 0, 0))
                strategy = self.driver.goToG(goal,
                                             TOLERATED_MISS,
                                             angleThreshold=ANGULAR_THRESHOLD,
                                             angularSpeed=ANGULAR_SPEED)
                prevDir = dir
                self.robot.toDisplay = 'OK'
                happyTime = self.robot.time

            if self.robot.time - happyTime > ENDURANCE:
                print "ENDURANCE!"
                break  # does not have sense to wait - try to turn somewhere else ...
                # We are stuck for too long. Let's try to retreat a bit:
                #    1) to show we are alive,
                #    2) to hope the robot gets out of the blocked state.
                # Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
                pose = self.robot.localisation.pose()
                goal = combinedPose(pose, (-(RETREAT + TOLERATED_MISS), 0, 0))
                strategy = self.driver.goToG(goal,
                                             TOLERATED_MISS,
                                             backward=True,
                                             angleThreshold=ANGULAR_THRESHOLD,
                                             angularSpeed=ANGULAR_SPEED)
                self.robot.toDisplay = '\/'
                for (speed, angularSpeed) in strategy:
                    self.robot.setSpeedPxPa(speed, angularSpeed)
                    self.robot.update()
                    distTraveled += self.robot.lastDistStep
                happyTime = self.robot.time
                continue  # TODO maybe replace by return -> give up

            for (speed, angularSpeed) in strategy:
                self.robot.setSpeedPxPa(speed, angularSpeed)
                self.robot.update()
                distTraveled += self.robot.lastDistStep
                pose = self.robot.localisation.pose()
                if pathFinder.newData:
                    pathFinder.newData = False
                    break

        self.robot.removeExtension("VHF")
        self.robot.removeExtension("PATH")
        print "... traveled", distTraveled, "time", self.robot.time - startTime