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_api(self):
# Test the containers' constructors and accessors.
srh = [4., 5., 6.]
lane_pos = LanePosition(srh[0], srh[1], srh[2])
self.assertTrue(len(lane_pos.srh()) == 3)
self.assertTrue(np.allclose(lane_pos.srh(), srh))
lane_pos_alt = LanePosition(s=4., r=5., h=6.)
self.assertTrue(np.allclose(lane_pos_alt.srh(), srh))
xyz = [1., 2., 3.]
geo_pos = GeoPosition(xyz[0], xyz[1], xyz[2])
self.assertTrue(len(geo_pos.xyz()) == 3)
self.assertTrue(np.allclose(geo_pos.xyz(), xyz))
geo_pos_alt = GeoPosition(x=1., y=2., z=3.)
self.assertTrue(np.allclose(geo_pos_alt.xyz(), xyz))
# Check that the getters are read-only.
with self.assertRaises(ValueError):
lane_pos.srh()[0] = 0.
with self.assertRaises(ValueError):
geo_pos.xyz()[0] = 0.
# Test RoadGeometryId accessors.
string = "foo"
rg_id = RoadGeometryId(string)
self.assertTrue(rg_id.string() == string)
def test_dragway(self):
kNumLanes = 2
kLength = 100.
kLaneWidth = 4.
kShoulderWidth = 1.
kHeight = 5.
kTol = 1e-6
# Instantiate a two-lane straight road.
rg_id = RoadGeometryId("two_lane_road")
rg = create_dragway(rg_id, kNumLanes, kLength, kLaneWidth,
kShoulderWidth, kHeight, kTol, kTol)
segment = rg.junction(0).segment(0)
lane_0 = segment.lane(0)
lane_1 = segment.lane(1)
# Alternate constructor.
create_dragway(road_id=rg_id,
num_lanes=kNumLanes,
length=kLength,
lane_width=kLaneWidth,
shoulder_width=kShoulderWidth,
maximum_height=kHeight,
linear_tolerance=kTol,
angular_tolerance=kTol)
# Test the Lane <-> Geo space coordinate conversion.
lane_pos = LanePosition(0., 0., 0.)
geo_pos_result = lane_0.ToGeoPosition(lane_pos)
geo_pos_expected = GeoPosition(0., -kLaneWidth / 2., 0.)
self.assertTrue(
np.allclose(geo_pos_result.xyz(), geo_pos_expected.xyz()))
geo_pos = GeoPosition(1., kLaneWidth / 2., 3.)
nearest_pos = GeoPosition(0., 0., 0.)
distance = 0.
lane_pos_result = \
lane_1.ToLanePosition(geo_pos, nearest_pos, distance)
lane_pos_expected = LanePosition(1., 0., 3.)
self.assertTrue(
np.allclose(lane_pos_result.srh(), lane_pos_expected.srh()))
self.assertTrue(np.allclose(nearest_pos.xyz(), geo_pos.xyz()))
self.assertTrue(distance == 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()))
def test_pose_selector(self):
kScanDistance = 4.
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pose0 = PoseVector()
pose0.set_translation([1., 0., 0.])
pose1 = PoseVector()
# N.B. Set pose1 3 meters ahead of pose0.
pose1.set_translation([4., 0., 0.])
bundle = PoseBundle(num_poses=2)
bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation()))
bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation()))
closest_pose = PoseSelector.FindSingleClosestPose(
lane=lane,
ego_pose=pose0,
traffic_poses=bundle,
scan_distance=kScanDistance,
side=AheadOrBehind.kAhead,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane.id().string())
self.assertTrue(closest_pose.distance == 3.)
closest_pair = PoseSelector.FindClosestPair(
lane=lane,
ego_pose=pose0,
traffic_poses=bundle,
scan_distance=kScanDistance,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(
closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.)
self.assertEqual(
closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance,
float('inf'))
lane_pos = LanePosition(s=1., r=0., h=0.)
road_pos = RoadPosition(lane=lane, pos=lane_pos)
w = [1., 2., 3.]
v = [-4., -5., -6.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
road_odom = RoadOdometry(road_position=road_pos,
frame_velocity=frame_velocity)
sigma_v = PoseSelector.GetSigmaVelocity(road_odom)
self.assertEqual(sigma_v, v[0])
def test_dragway(self):
kLaneWidth = 4.
rg = make_test_dragway(kLaneWidth=kLaneWidth)
segment = rg.junction(0).segment(0)
lane_0 = segment.lane(0)
lane_1 = segment.lane(1)
# Test the Lane <-> Geo space coordinate conversion.
lane_pos = LanePosition(0., 0., 0.)
geo_pos_result = lane_0.ToGeoPosition(lane_pos)
geo_pos_expected = GeoPosition(0., -kLaneWidth / 2., 0.)
self.assertTrue(np.allclose(geo_pos_result.xyz(),
geo_pos_expected.xyz()))
geo_pos = GeoPosition(1., kLaneWidth / 2., 3.)
nearest_pos = GeoPosition(0., 0., 0.)
distance = 0.
lane_pos_result = \
lane_1.ToLanePosition(geo_pos, nearest_pos, distance)
lane_pos_expected = LanePosition(1., 0., 3.)
self.assertTrue(np.allclose(lane_pos_result.srh(),
lane_pos_expected.srh()))
self.assertTrue(np.allclose(nearest_pos.xyz(), geo_pos.xyz()))
self.assertTrue(distance == 0.)
def test_lane_direction(self):
rg = make_two_lane_road()
lane1 = rg.junction(0).segment(0).lane(0)
lane_direction = LaneDirection(lane=lane1, with_s=True)
test_coordinate = LanePosition(s=1., r=2., h=3.)
self.assertTrue(np.allclose(
lane_direction.lane.ToGeoPosition(test_coordinate).xyz(),
lane1.ToGeoPosition(test_coordinate).xyz()))
self.assertEqual(lane_direction.with_s, True)
lane2 = rg.junction(0).segment(0).lane(1)
lane_direction.lane = lane2
lane_direction.with_s = False
self.assertTrue(np.allclose(
lane_direction.lane.ToGeoPosition(test_coordinate).xyz(),
lane2.ToGeoPosition(test_coordinate).xyz()))
self.assertEqual(lane_direction.with_s, False)
def test_dragway(self):
kLaneWidth = 4.
kLength = 100.
rg = make_test_dragway(lane_width=kLaneWidth,
length=kLength)
lane_0 = rg.junction(0).segment(0).lane(0)
lane_1 = rg.junction(0).segment(0).lane(1)
# Test that both Lanes have equal and expected lengths.
self.assertEqual(lane_0.length(), kLength)
self.assertEqual(lane_0.length(), lane_1.length())
# Test Lane orientations for consistency.
lane_start = LanePosition(0., 0., 0.)
lane_end = LanePosition(lane_0.length(), 0., 0.)
self.assertTrue(np.allclose(
lane_0.GetOrientation(lane_start).quat().wxyz(),
[1., 0., 0., 0.]
))
self.assertTrue(np.allclose(
lane_0.GetOrientation(lane_start).quat().wxyz(),
lane_0.GetOrientation(lane_end).quat().wxyz()
))
self.assertTrue(np.allclose(
lane_1.GetOrientation(lane_start).quat().wxyz(),
lane_1.GetOrientation(lane_end).quat().wxyz()
))
self.assertTrue(np.allclose(
lane_0.GetOrientation(lane_end).quat().wxyz(),
lane_1.GetOrientation(lane_end).quat().wxyz()
))
# Test the Lane <-> Geo space coordinate conversion.
lane_pos = LanePosition(0., 0., 0.)
geo_pos_result = lane_0.ToGeoPosition(lane_pos)
geo_pos_expected = GeoPosition(0., -kLaneWidth / 2., 0.)
self.assertTrue(np.allclose(geo_pos_result.xyz(),
geo_pos_expected.xyz()))
geo_pos = GeoPosition(1., kLaneWidth / 2., 3.)
nearest_pos = GeoPosition(0., 0., 0.)
distance = 0.
lane_pos_result = \
lane_1.ToLanePosition(geo_pos, nearest_pos, distance)
lane_pos_expected = LanePosition(1., 0., 3.)
self.assertTrue(np.allclose(lane_pos_result.srh(),
lane_pos_expected.srh()))
self.assertTrue(np.allclose(nearest_pos.xyz(), geo_pos.xyz()))
self.assertTrue(distance == 0.)
def test_closest_pose(self):
ClosestPose()
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)
closest_pose = ClosestPose(odom=road_odom, dist=12.7)
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane_0.id().string())
self.assertEqual(closest_pose.distance, 12.7)
closest_pose.odometry.lane = lane_1
closest_pose.distance = 5.9
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane_1.id().string())
self.assertEqual(closest_pose.distance, 5.9)
def test_api(self):
# Test the containers' constructors and accessors.
srh = [4., 5., 6.]
lane_pos = LanePosition(srh[0], srh[1], srh[2])
self.assertTrue(len(lane_pos.srh()) == 3)
self.assertTrue(np.allclose(lane_pos.srh(), srh))
lane_pos_alt = LanePosition(s=4., r=5., h=6.)
self.assertTrue(np.allclose(lane_pos_alt.srh(), srh))
xyz = [1., 2., 3.]
geo_pos = GeoPosition(xyz[0], xyz[1], xyz[2])
self.assertTrue(len(geo_pos.xyz()) == 3)
self.assertTrue(np.allclose(geo_pos.xyz(), xyz))
geo_pos_alt = GeoPosition(x=1., y=2., z=3.)
self.assertTrue(np.allclose(geo_pos_alt.xyz(), xyz))
RoadPosition()
rg = make_test_dragway(lane_width=4., length=100.)
self.assertEqual(rg.num_junctions(), 1)
junction = rg.junction(0)
self.assertEqual(junction.num_segments(), 1)
segment = junction.segment(0)
self.assertEqual(segment.num_lanes(), 2)
lane_0 = segment.lane(0)
lane_1 = segment.lane(1)
road_pos = RoadPosition(lane=lane_0, pos=lane_pos)
self.assertEqual(road_pos.lane.id().string(), lane_0.id().string())
self.assertTrue(np.allclose(road_pos.pos.srh(), lane_pos.srh()))
road_pos.lane = lane_1
self.assertEqual(road_pos.lane.id().string(), lane_1.id().string())
new_srh = [42., 43., 44.]
road_pos.pos = LanePosition(s=new_srh[0], r=new_srh[1], h=new_srh[2])
self.assertTrue(np.allclose(road_pos.pos.srh(), new_srh))
lane_orientation = lane_0.GetOrientation(lane_pos)
self.assertTrue(np.allclose(
lane_orientation.rpy().ToQuaternion().wxyz(),
lane_orientation.quat().wxyz(),
))
# Check that the getters are read-only.
with self.assertRaises(ValueError):
lane_pos.srh()[0] = 0.
with self.assertRaises(ValueError):
geo_pos.xyz()[0] = 0.
# Test RoadGeometryId accessors.
string = "foo"
rg_id = RoadGeometryId(string)
self.assertEqual(rg_id.string(), string)