コード例 #1
0
ファイル: cube_test.py プロジェクト: robotika/eduro
    def test_badly_loaded_cube(self):
        # logs\2016-10-07\czu1\src_laser_161007_115410.log
        # diagonally loaded cube
        data = [227, 229, 241, 245, 236, 246, 252, 261, 265, 272, 279, 280, 280, 282, 273,
        261, 257, 258, 252, 250, 247, 237, 238, 242, 242, 306, 422, 419, 430,
        423, 426, 430, 425, 431, 448, 446, 442, 444, 448, 2, 6469, 6506, 6583,
        6293, 6200, 6316, 6354, 4475, 4412, 4280, 4164, 2, 0, 3872, 0, 5363,
        153, 160, 166, 164, 157, 160, 155, 154, 149, 149, 146, 143, 146, 145,
        141, 136, 136, 129, 130, 127, 119, 119, 113, 108, 103, 102, 103, 106,
        108, 106, 105, 105, 98, 95, 92, 85, 71, 69, 66, 67, 71, 65, 71, 58, 57,
        66, 65, 54, 53, 52, 58, 57, 58, 48, 58, 54, 54, 56, 55, 53, 51, 43, 64,
        73, 94, 143, 156, 0, 0, 0, 0, 5735, 0, 7207, 0, 0, 0, 4756, 4680, 4709,
        0, 5333, 5372, 5427, 5313, 4897, 4906, 5055, 5101, 4538, 4545, 5312,
        4808, 0, 5474, 4168, 4204, 4192, 4164, 4128, 4074, 4052, 4014, 3990,
        3961, 3852, 3965, 2492, 730, 671, 658, 653, 666, 655, 664, 666, 679,
        669, 688, 689, 694, 688, 0, 6120, 6129, 6098, 6084, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 580, 592, 588,
        585, 579, 574, 563, 572, 563, 564, 558, 556, 561, 547, 562, 660, 0, 0,
        2, 4336, 4134, 4084, 4122, 4256, 4177, 4183, 4114, 5784, 4035, 0, 2, 2,
        3656, 3634, 3595, 3513, 3472, 3403, 3374, 3371, 3365, 3367, 3406, 3412,
        3417, 3442, 3437, 0, 2617, 2648, 0, 3529, 3577, 3576, 3418, 3419, 2173,
        2187, 2233, 2586, 2547, 2510, 2470]

        loaded, blocked_view_index = verify_loaded_cube(data)
        self.assertTrue(loaded)
コード例 #2
0
ファイル: sickday2016.py プロジェクト: robotika/eduro
    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()