def test_pure_pursuit(self):
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pure_pursuit = PurePursuitController()
context = pure_pursuit.CreateDefaultContext()
output = pure_pursuit.AllocateOutput()
# Fix the inputs.
ld_value = framework.AbstractValue.Make(
LaneDirection(lane=lane, with_s=True))
lane_index = pure_pursuit.lane_input().get_index()
context.FixInputPort(lane_index, ld_value)
pos = [1., 2., 3.] # An aribtrary position with the lane.
pose_vector = PoseVector()
pose_vector.set_translation(pos)
pose_index = pure_pursuit.ego_pose_input().get_index()
context.FixInputPort(pose_index, pose_vector)
# Verify the inputs.
pose_vector_eval = pure_pursuit.EvalVectorInput(context, pose_index)
self.assertTrue(np.allclose(pose_vector.get_translation(),
pose_vector_eval.get_translation()))
# Verify the outputs.
pure_pursuit.CalcOutput(context, output)
command_index = pure_pursuit.steering_command_output().get_index()
steering = output.get_vector_data(command_index)
self.assertEqual(len(steering.get_value()), 1)
self.assertTrue(steering.get_value() < 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_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)