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()))
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()))
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_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))