Пример #1
0
    def test_find_goal(
            self):  # only functions with 'test_'-prefix will be run!
        # Set up path
        pp = PurePursuit()
        pp.path = Path()

        pose0 = PoseStamped()
        pose0.pose.position.x = 0.0
        pose0.pose.position.y = 0.0
        pp.path.poses.append(pose0)

        pose1 = PoseStamped()
        pose1.pose.position.x = 1.0
        pose1.pose.position.y = 0.0
        pp.path.poses.append(pose1)

        pose2 = PoseStamped()
        pose2.pose.position.x = 2.0
        pose2.pose.position.y = 1.0
        pp.path.poses.append(pose2)

        pp.lookahead = 1.0

        # Set up test inputs
        x = [np.array([-0.25, 0.0]), np.array([0.5, 0.]), np.array([1.5, 0.5])]

        # Desired outputs
        goals_true = [
            np.array([0.75, 0.]),
            np.array([1.41143782777, 0.411437827766]),
            np.array([2., 1.])
        ]

        # Ensure that calculated outputs match desired outputs
        err_msg = "test point ({},{}) has the wrong goal ({}, {}) instead of ({}, {})"
        for i in range(0, len(x)):
            (pt, dist, seg) = pp.find_closest_point(x[i])
            goal = pp.find_goal(x[i], pt, dist, seg)
            self.assertTrue(
                np.linalg.norm(goal - goals_true[i]) < 1e-6,
                err_msg.format(x[i][0], x[i][1], goal[0], goal[1],
                               goals_true[i][0], goals_true[i][1]))
Пример #2
0
    def test_find_closest_point(
            self):  # only functions with 'test_'-prefix will be run!
        # Set up path
        pp = PurePursuit()
        pp.path = Path()

        pose0 = PoseStamped()
        pose0.pose.position.x = 0.0
        pose0.pose.position.y = 0.0
        pp.path.poses.append(pose0)

        pose1 = PoseStamped()
        pose1.pose.position.x = 1.0
        pose1.pose.position.y = 0.0
        pp.path.poses.append(pose1)

        pose2 = PoseStamped()
        pose2.pose.position.x = 2.0
        pose2.pose.position.y = 1.0
        pp.path.poses.append(pose2)

        # Set up test inputs
        x = [
            np.array([-1., 0.]),
            np.array([0., 0.1]),
            np.array([0.5, 0.5]),
            np.array([0.9, -1.]),
            np.array([1.5, 0.5]),
            np.array([2., 0.5]),
            np.array([3., 3.])
        ]
        #x = np.array([-1.,  0., 0.5, 0.9, 1.5, 2.0, 3.0])
        #y = np.array([ 0., 0.1, 0.5, -1., 0.5, 0.5, 3.0])

        # Desired outputs
        pts_true = [
            np.array([0., 0.]),
            np.array([0., 0.]),
            np.array([0.5, 0.]),
            np.array([0.9, 0.]),
            np.array([1.5, 0.5]),
            np.array([1.75, 0.75]),
            np.array([2., 1.])
        ]
        dists_true = [1., 0.1, 0.5, 1.0, 0., 0.25 * np.sqrt(2.), np.sqrt(5.)]
        segs_true = [0, 0, 0, 0, 1, 1, 1]

        # Ensure that calculated outputs match desired outputs
        err_msg_pt = "test point ({},{}) has the wrong closest point ({}, {}) instead of ({}, {})"
        err_msg_dist = "test point ({},{}) has the wrong distance ({} instead of {})"
        err_msg_seg = "test point ({},{}) has the wrong segment ({} instead of {})"
        for i in range(0, len(x)):
            (pt, dist, seg) = pp.find_closest_point(x[i])
            self.assertTrue(
                np.linalg.norm(pt - pts_true[i]) < 1e-6,
                err_msg_pt.format(x[i][0], x[i][1], pt[0], pt[1],
                                  pts_true[i][0], pts_true[i][1]))
            self.assertTrue(
                np.abs(dist - dists_true[i]) < 1e-6,
                err_msg_dist.format(x[i][0], x[i][1], dist, dists_true[i]))
            self.assertEqual(
                seg, segs_true[i],
                err_msg_seg.format(x[i][0], x[i][1], seg, segs_true[i]))