Example #1
0
    def test_followLineG( self ):
        robot = MagicMock()
        robot.localisation.pose.return_value = (0, 0.3, 0)
        driver = Driver(robot, maxSpeed = 0.5)
        offsetDistance = 0.03
        gen = driver.followPolyLineG(pts = [(0, 0), (1.0, 0)],
                                     offsetDistance=offsetDistance)
        speed, angularSpeed = gen.next()
        self.assertAlmostEqual(angularSpeed, -4 * math.radians(20))
        self.assertAlmostEqual(speed, 0.444444444444)

        # now for closer distance reduce the turning angle
        robot.localisation.pose.return_value = (0, 1.5 * offsetDistance, 0)
        speed, angularSpeed = gen.next()
        self.assertAlmostEqual(angularSpeed, -4 * math.radians(10))
        self.assertAlmostEqual(speed, 0.4722222222222222)
Example #2
0
class SICKRobotDay2016:
    def __init__(self, robot, code, verbose = False):
        self.random = random.Random(0).uniform 
        self.robot = robot
        self.verbose = verbose
        self.code = code
        self.robot.attachEmergencyStopButton()

        # change robot wheel calibration
        wd = 427.0 / 445.0 * 0.26/4.0 # with gear  1:4
        delta = -0.00015
        self.robot._WHEEL_DIAMETER = {SIDE_LEFT: wd+delta, SIDE_RIGHT: wd-delta}

        self.robot.attachCamera(sleep=0.5)
        self.robot.attachLaser( remission=True, pose=((0.24, -0.13, 0.08), (0,math.radians(180),0)) )
        self.robot.attachRFU620()
        
        self.driver = Driver( self.robot, maxSpeed = 0.5, maxAngularSpeed = math.radians(180) )
        self.robot.localisation = SimpleOdometry()

        self.robot.last_valid_rfid = None, None  # tag, position
        self.robot.addExtension(draw_rfu620_extension)

        self.robot.addExtension(draw_cubes_extension)

        self.robot.laser.stopOnExit = False    # for faster boot-up


    def load_cube(self):
        print "load cube (time = {:.1f}s)".format(self.robot.time - self.start_time)
        self.driver.goStraight(0.3, timeout=30)
        gripperClose(self.robot)

    def place_cube(self):
        pos = combinedPose(self.robot.localisation.pose(), (0.2, 0, 0))[:2]  # TODO check proper offset
        print "place cube at ({:.2f}, {:.2f} (time = {:.1f}))".format(pos[0], pos[1], self.robot.time - self.start_time)
        viewlog.dumpBeacon(pos, color=(255, 255, 255))
        viewlog.dumpObstacles([[pos]])
        gripperOpen(self.robot)
        self.driver.goStraight(-0.4, timeout=30)

    def game_over(self):
        print 'Game over (battery status = {}V)'.format(self.robot.battery)
        for k in xrange(10):
            self.robot.setSpeedPxPa(0.0, 0.0)
            self.robot.update()
        raise EmergencyStopException() # TODO: Introduce GameOverException as in Eurobot


    def find_cube(self, timeout):
        print "find_cube-v1"
        prev = None
        cd = CubeDetector(self.robot.laser.pose)
        startTime = self.robot.time
        while self.robot.time < startTime + timeout:
            self.robot.update()
            if prev != self.robot.laserData:
                prev = self.robot.laserData
                cubes = cd.detect_cubes_xy(self.robot.laserData, limit=4)
                if len(cubes) > 0:
                    for i, (cube_x, cube_y) in enumerate(cubes):
                        goal = combinedPose(self.robot.localisation.pose(), (cube_x, cube_y, 0))[:2]
                        viewlog.dumpBeacon(goal, color=(200, 200, 0) if i > 0 else (255, 255, 0))
                    cube_x, cube_y = cubes[0]
                    cube_y += 0.05  # hack for bended(?) laser
                    print "{:.2f}\t{:.2f}".format(cube_x, cube_y)
                    speed = 0.0

                    tolerance = 0.01
                    if cube_x > 0.5:
                        tolerance = 0.05

                    angle = math.atan2(cube_y, cube_x)
                    turnStep = math.radians(10)
                    if abs(angle) > math.radians(10):
                        turnStep = math.radians(50)

                    if cube_y > tolerance:
                        angularSpeed = turnStep
                    elif cube_y < -tolerance:
                        angularSpeed = -turnStep
                    else:
                        angularSpeed = 0
                        speed = 0.1
                        if cube_x > 0.5:
                            speed = 0.3  # move faster toward further goals
                        if cube_x < 0.30:
                            return True
                    self.robot.setSpeedPxPa(speed, angularSpeed)
                else:
                    self.robot.setSpeedPxPa(0.0, 0.0)

        print "TIMEOUT"
        return False



    def ver0( self, verbose=False ):
        # Go straight for 2 meters
        print "ver0", self.robot.battery

        prevRFID = None
        self.load_cube()
        for cmd in self.driver.goStraightG(2.0):
            self.robot.setSpeedPxPa(*cmd)

            if prevRFID != self.robot.rfu620Data:
                prevRFID = self.robot.rfu620Data
                posXY = combinedPose(self.robot.localisation.pose(), (-0.35, 0.14, 0))[:2]
                for d in self.robot.rfu620Data[1]:
                    i = d[0]  # i.e. 0x1000 0206 0000
                    x, y, zone = (i >> 24)&0xFF, (i >> 16)&0xFF, i&0xFF
                    print hex(i), (x, y)
                    if x == 1:
                        viewlog.dumpBeacon(posXY, color=(0, 0, 255))
                    elif x == 2:
                        viewlog.dumpBeacon(posXY, color=(255, 0, 255))
                    else:
                        viewlog.dumpBeacon(posXY, color=(255, 255, 255))
            self.robot.update()
        self.place_cube()
        self.game_over()


    def handle_single_cube(self, pts, rev_pts, timeout=60):
        if self.find_cube(timeout=20.0):
            self.load_cube()
            print verify_loaded_cube(self.robot.laserData)

        start_time = self.robot.time
        for cmd in self.driver.followPolyLineG(pts, withStops=True):
            self.robot.setSpeedPxPa(*cmd)
            self.robot.update()
            if (not is_in_loading_zone(self.robot.localisation.pose(),
                                       self.robot.last_valid_rfid)
                and is_path_blocked(self.robot.laserData, self.robot.remissionData)):
                print "ESCAPING FROM", self.robot.localisation.pose()
                self.driver.stop()
                break
            if self.robot.time - start_time > timeout:
                print "TIMEOUT", self.robot.time - start_time
                self.driver.goStraight(-0.3, timeout=10)
                self.driver.turn(angle=math.radians(90), timeout=10)
                self.driver.turn(angle=math.radians(-90), timeout=10)
                self.place_cube()
                return  # demo - game3 experiment
        self.place_cube()

        # handle offset in case of blocked path
        print rev_pts
        route = Route(pts=rev_pts, conv=DummyConvertor())
        _, dist, signed_index = route.findNearestEx(self.robot.localisation.pose())
        rev_pts = rev_pts[max(0, abs(signed_index) - 1):]
        print "cut path", dist, signed_index, rev_pts

        self.driver.turn(angle=math.radians(180), timeout=30)

        for cmd in self.driver.followPolyLineG(rev_pts):
            self.robot.setSpeedPxPa(*cmd)
            self.robot.update()


    def ver1(self, verbose=False):
        # Navigate on polyline
        print "ver1", self.robot.battery, self.driver.maxAngularSpeed
        self.driver.maxAngularSpeed = math.pi/2.0

        paths = [
            ( [(1.5, 2.5), (1.5, 1.5), (6.5, 1.5)], [(6.5, 1.5), (3.5, 1.5)] ),
            ( [(3.5, 2.5), (5.5, 2.5)], [(5.5, 2.5), (3.5, 2.5)] ),
            ( [(3.5, 3.5), (6.5, 3.5)], [(6.5, 3.5), (3.5, 3.5)] ),
            ( [(3.5, 4.5), (5.5, 4.5)], [(5.5, 4.5), (3.5, 4.5)] ),
            ( [(3.5, 5.5), (6.5, 5.5)], [(6.5, 5.5), (3.5, 5.5)] ),

            ( [(3.5, 5.5), (5.5, 5.5)], [(5.5, 5.5), (3.5, 5.5)] ),
            ( [(3.5, 4.5), (4.5, 4.5)], [(4.5, 4.5), (3.5, 4.5)] ),
            ( [(3.5, 3.5), (5.5, 3.5)], [(5.5, 3.5), (3.5, 3.5)] ),
            ( [(3.5, 2.5), (4.5, 2.5)], [(4.5, 2.5), (3.5, 2.5)] ),
            ( [(3.5, 1.5), (5.5, 1.5)], [(5.5, 1.5), (3.5, 1.5)] ),

            ( [(3.5, 1.5), (4.5, 1.5)], [(4.5, 1.5), (3.5, 1.5)] ),
            ( [(3.5, 3.5), (4.5, 3.5)], [(4.5, 3.5), (3.5, 3.5)] ),
            ( [(3.5, 5.5), (4.5, 5.5)], [(4.5, 5.5), (3.5, 5.5)] ),
        ]
        self.robot.localisation.setPose( (1.5, 2.2, 0.0) )
        for path, rev_path in paths:
            self.handle_single_cube(path, rev_path)
        self.game_over()


    def test_line( self, verbose=False ):
        print "test_line", self.robot.battery
        self.driver.goStraight(10.0, timeout=60)
        self.game_over()

    def test_square( self, verbose=False ):
        print "test_square", self.robot.battery
        while True:
            self.driver.turn(angle=math.radians(90), timeout=30)
            self.driver.goStraight(1.0, timeout=30)


    def test_pick_cube(self, verbose=False):
        print "test_pick_cube", self.robot.battery
        if self.find_cube(timeout=20.0):
            self.load_cube()
        self.driver.goStraight(0.5, timeout=10)
        self.place_cube()
        self.game_over()


    def run( self ):
        self.start_time = float('nan')  # just to be defined in case of EmergencyStopException
        try:
            if getattr( self.robot.laser, 'startLaser', None ):
                # trigger rotation of the laser, low level function, ignore for log files
                print "Powering laser ON"
                self.robot.laser.startLaser() 
            gripperOpen(self.robot)
            self.robot.waitForStart()
            self.start_time = self.robot.time

            self.robot.laser.start()    # laser also after start -- it should be already running
            self.robot.camera.start()
            self.robot.rfu620.start()
            self.robot.localisation = SimpleOdometry()

            while True:
#                self.ver0(verbose = self.verbose)
                self.ver1(verbose = self.verbose)
#                self.test_line(verbose = self.verbose)

        except EmergencyStopException, e:
            print "EmergencyStopException at {} sec".format(self.robot.time - self.start_time)
        gripperDisableServos(self.robot.can)
        self.robot.laser.requestStop()
        self.robot.rfu620.requestStop()
        self.robot.camera.requestStop()