Beispiel #1
0
    def test_road_odometry(self):
        RoadOdometry()
        rg = make_two_lane_road()
        lane_0 = rg.junction(0).segment(0).lane(0)
        lane_1 = rg.junction(0).segment(0).lane(1)
        lane_pos = LanePosition(s=1., r=2., h=3.)
        road_pos = RoadPosition(lane=lane_0, pos=lane_pos)
        w = [5., 7., 9.]
        v = [11., 13., 15.]
        frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
        road_odom = RoadOdometry(
            road_position=road_pos, frame_velocity=frame_velocity)

        self.assertEqual(road_odom.lane.id().string(), lane_0.id().string())
        self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos.srh()))
        self.assertTrue(np.allclose(
            frame_velocity.get_velocity().translational(),
            road_odom.vel.get_velocity().translational()))
        lane_pos_new = LanePosition(s=10., r=20., h=30.)
        v_new = [42., 43., 44.]
        frame_velocity_new = FrameVelocity(SpatialVelocity(w=w, v=v_new))
        road_odom.lane = lane_1
        road_odom.pos = lane_pos_new
        road_odom.vel = frame_velocity_new

        self.assertEqual(road_odom.lane.id().string(), lane_1.id().string())
        self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos_new.srh()))
        self.assertTrue(np.allclose(
            frame_velocity_new.get_velocity().translational(),
            road_odom.vel.get_velocity().translational()))
Beispiel #2
0
    def test_road_odometry(self):
        RoadOdometry()
        rg = make_two_lane_road()
        lane_0 = rg.junction(0).segment(0).lane(0)
        lane_1 = rg.junction(0).segment(0).lane(1)
        lane_pos = LanePosition(s=1., r=2., h=3.)
        road_pos = RoadPosition(lane=lane_0, pos=lane_pos)
        w = [5., 7., 9.]
        v = [11., 13., 15.]
        frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
        road_odom = RoadOdometry(
            road_position=road_pos, frame_velocity=frame_velocity)

        self.assertEqual(road_odom.lane.id().string(), lane_0.id().string())
        self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos.srh()))
        self.assertTrue(np.allclose(
            frame_velocity.get_velocity().translational(),
            road_odom.vel.get_velocity().translational()))
        lane_pos_new = LanePosition(s=10., r=20., h=30.)
        v_new = [42., 43., 44.]
        frame_velocity_new = FrameVelocity(SpatialVelocity(w=w, v=v_new))
        road_odom.lane = lane_1
        road_odom.pos = lane_pos_new
        road_odom.vel = frame_velocity_new

        self.assertEqual(road_odom.lane.id().string(), lane_1.id().string())
        self.assertTrue(np.allclose(road_odom.pos.srh(), lane_pos_new.srh()))
        self.assertTrue(np.allclose(
            frame_velocity_new.get_velocity().translational(),
            road_odom.vel.get_velocity().translational()))
Beispiel #3
0
    def test_idm_controller(self):
        rg = make_two_lane_road()
        idm = IdmController(
            road=rg,
            path_or_branches=ScanStrategy.kPath,
            road_position_strategy=RoadPositionStrategy.kExhaustiveSearch,
            period_sec=0.)
        context = idm.CreateDefaultContext()
        output = idm.AllocateOutput()

        # Fix the inputs.
        pose_vector1 = PoseVector()
        pose_vector1.set_translation([1., 2., 3.])
        ego_pose_index = idm.ego_pose_input().get_index()
        context.FixInputPort(ego_pose_index, pose_vector1)

        w = [0., 0., 0.]
        v = [1., 0., 0.]
        frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
        ego_velocity_index = idm.ego_velocity_input().get_index()
        context.FixInputPort(ego_velocity_index, frame_velocity1)

        pose_vector2 = PoseVector()
        pose_vector2.set_translation([6., 0., 0.])
        bundle = PoseBundle(num_poses=1)
        bundle.set_pose(
            0, Isometry3(Quaternion(), pose_vector2.get_translation()))
        traffic_index = idm.traffic_input().get_index()
        context.FixInputPort(traffic_index,
                             framework.AbstractValue.Make(bundle))

        # Verify the inputs.
        pose_vector_eval = idm.EvalVectorInput(context, 0)
        self.assertTrue(
            np.allclose(pose_vector1.get_translation(),
                        pose_vector_eval.get_translation()))
        frame_velocity_eval = idm.EvalVectorInput(context, 1)
        self.assertTrue(
            np.allclose(frame_velocity1.get_velocity().translational(),
                        frame_velocity_eval.get_velocity().translational()))
        self.assertTrue(
            np.allclose(frame_velocity1.get_velocity().rotational(),
                        frame_velocity_eval.get_velocity().rotational()))

        # Verify the outputs.
        idm.CalcOutput(context, output)
        accel_command_index = idm.acceleration_output().get_index()
        accel = output.get_vector_data(accel_command_index)
        self.assertEqual(len(accel.get_value()), 1)
        self.assertTrue(accel.get_value() < 0.)
Beispiel #4
0
    def test_idm_controller(self):
        rg = make_two_lane_road()
        idm = IdmController(
            road=rg, path_or_branches=ScanStrategy.kPath,
            road_position_strategy=RoadPositionStrategy.kExhaustiveSearch,
            period_sec=0.)
        context = idm.CreateDefaultContext()
        output = idm.AllocateOutput()

        # Fix the inputs.
        pose_vector1 = PoseVector()
        pose_vector1.set_translation([1., 2., 3.])
        ego_pose_index = idm.ego_pose_input().get_index()
        context.FixInputPort(ego_pose_index, pose_vector1)

        w = [0., 0., 0.]
        v = [1., 0., 0.]
        frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
        ego_velocity_index = idm.ego_velocity_input().get_index()
        context.FixInputPort(ego_velocity_index, frame_velocity1)

        pose_vector2 = PoseVector()
        pose_vector2.set_translation([6., 0., 0.])
        bundle = PoseBundle(num_poses=1)
        bundle.set_pose(
            0, Isometry3(Quaternion(), pose_vector2.get_translation()))
        traffic_index = idm.traffic_input().get_index()
        context.FixInputPort(
            traffic_index, framework.AbstractValue.Make(bundle))

        # Verify the inputs.
        pose_vector_eval = idm.EvalVectorInput(context, 0)
        self.assertTrue(np.allclose(pose_vector1.get_translation(),
                                    pose_vector_eval.get_translation()))
        frame_velocity_eval = idm.EvalVectorInput(context, 1)
        self.assertTrue(np.allclose(
            frame_velocity1.get_velocity().translational(),
            frame_velocity_eval.get_velocity().translational()))
        self.assertTrue(np.allclose(
            frame_velocity1.get_velocity().rotational(),
            frame_velocity_eval.get_velocity().rotational()))

        # Verify the outputs.
        idm.CalcOutput(context, output)
        accel_command_index = idm.acceleration_output().get_index()
        accel = output.get_vector_data(accel_command_index)
        self.assertEqual(len(accel.get_value()), 1)
        self.assertTrue(accel.get_value() < 0.)
 def test_frame_velocity(self):
     frame_velocity = FrameVelocity()
     self.assertTrue(isinstance(frame_velocity, BasicVector))
     self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity))
     self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity))
     self.assertEqual(frame_velocity.size(), FrameVelocity.kSize)
     # - Accessors.
     self.assertTrue(
         isinstance(frame_velocity.get_velocity(), SpatialVelocity))
     # - Value.
     self.assertTrue(
         np.allclose(frame_velocity.get_velocity().rotational(), 3 * [0.]))
     self.assertTrue(
         np.allclose(frame_velocity.get_velocity().translational(),
                     3 * [0.]))
     # - Mutators.
     w = [0.1, 0.3, 0.5]
     v = [0., 1., 2.]
     frame_velocity.set_velocity(SpatialVelocity(w=w, v=v))
     self.assertTrue(
         np.allclose(frame_velocity.get_velocity().rotational(), w))
     self.assertTrue(
         np.allclose(frame_velocity.get_velocity().translational(), v))
     # - Ensure ordering is ((wx, wy, wz), (vx, vy, vz))
     velocity_expected = np.hstack((w, v))
     velocity_actual = frame_velocity.get_value()
     self.assertTrue(np.allclose(velocity_actual, velocity_expected))
     # - Fully-parameterized constructor.
     frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
     self.assertTrue(
         np.allclose(frame_velocity1.get_velocity().rotational(), w))
     self.assertTrue(
         np.allclose(frame_velocity1.get_velocity().translational(), v))
 def test_frame_velocity(self):
     frame_velocity = FrameVelocity()
     self.assertTrue(isinstance(frame_velocity, BasicVector))
     self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity))
     self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity))
     self.assertEqual(frame_velocity.size(), FrameVelocity.kSize)
     # - Accessors.
     self.assertTrue(isinstance(
         frame_velocity.get_velocity(), SpatialVelocity))
     # - Value.
     self.assertTrue(np.allclose(
         frame_velocity.get_velocity().rotational(), 3 * [0.]))
     self.assertTrue(np.allclose(
         frame_velocity.get_velocity().translational(), 3 * [0.]))
     # - Mutators.
     w = [0.1, 0.3, 0.5]
     v = [0., 1., 2.]
     frame_velocity.set_velocity(SpatialVelocity(w=w, v=v))
     self.assertTrue(np.allclose(
         frame_velocity.get_velocity().rotational(), w))
     self.assertTrue(np.allclose(
         frame_velocity.get_velocity().translational(), v))
     # - Ensure ordering is ((wx, wy, wz), (vx, vy, vz))
     velocity_expected = np.hstack((w, v))
     velocity_actual = frame_velocity.get_value()
     self.assertTrue(np.allclose(velocity_actual, velocity_expected))
     # - Fully-parameterized constructor.
     frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
     self.assertTrue(np.allclose(
         frame_velocity1.get_velocity().rotational(), w))
     self.assertTrue(np.allclose(
         frame_velocity1.get_velocity().translational(), v))