def get_past_and_future_lanes(self, pos, quat, lane_data):
# TODO: filter conditions needs some work
if len(lane_data['incoming_lane_data']) == 0:
lane_data['incoming_lane_data'].append({'record': None})
if len(lane_data['outgoing_lane_data']) == 0:
lane_data['outgoing_lane_data'].append({'record': None})
if lane_data['closest_lane_data']['record'] is None or lane_data[
'incoming_lane_data'][0]['record'] is None or lane_data[
'outgoing_lane_data'][0]['record'] is None:
past_lane = np.array([pos[:2]])
future_lanes = [np.array([pos[:2]])]
else:
c_lane = lane_data['closest_lane_data']['poses']
p_lane = lane_data['incoming_lane_data'][0]['poses']
f_lanes = [l['poses'] for l in lane_data['outgoing_lane_data']]
c_lane_local = convert_global_coords_to_local(
c_lane[:, :2], pos, quat)
p_lane_local = convert_global_coords_to_local(
p_lane[:, :2], pos, quat)
f_lane_local = [
convert_global_coords_to_local(l[:, :2], pos, quat)
for l in f_lanes
]
cp_lane = np.vstack([p_lane_local, c_lane_local])
cpf_lanes = [np.vstack([cp_lane, f]) for f in f_lane_local]
past_lane_idx = np.argmax(cp_lane[:, 1] > 0)
if past_lane_idx == 0:
past_lane = np.array([pos[:2]])
future_lanes = [np.array([pos[:2]])]
else:
cp_lane_gb = convert_local_coords_to_global(cp_lane, pos, quat)
cpf_lanes_gb = [
convert_local_coords_to_global(l, pos, quat)
for l in cpf_lanes
]
past_lane = cp_lane_gb[:past_lane_idx, :]
future_lanes = [l[past_lane_idx:, :] for l in cpf_lanes_gb]
past_lane = process_to_len(past_lane,
500,
name='past_lane',
dim=0,
before_or_after='before',
mode='edge')
future_lanes = [
process_to_len(l,
500,
name='future_lane',
dim=0,
before_or_after='after',
mode='edge') for l in future_lanes
]
return past_lane, future_lanes
def _run(
self,
rotation: Tuple[float, float, float, float],
origin: Tuple[float, float, float],
offset: Tuple[float, float, float],
along_pos_x_answer: np.ndarray,
along_pos_y_answer: np.ndarray,
along_neg_x_answer: np.ndarray,
along_neg_y_answer: np.ndarray,
) -> None:
offset_as_list = [[offset[0], offset[1]]]
# Testing path along pos x direction
answer = convert_global_coords_to_local(self.along_pos_x, origin,
rotation)
np.testing.assert_allclose(answer, along_pos_x_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_pos_x + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_pos_x_answer, atol=1e-4)
# Testing path along pos y direction
answer = convert_global_coords_to_local(self.along_pos_y, origin,
rotation)
np.testing.assert_allclose(answer, along_pos_y_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_pos_y + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_pos_y_answer, atol=1e-4)
# Testing path along neg x direction
answer = convert_global_coords_to_local(self.along_neg_x, origin,
rotation)
np.testing.assert_allclose(answer, along_neg_x_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_neg_x + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_neg_x_answer, atol=1e-4)
# Testing path along neg y direction
answer = convert_global_coords_to_local(self.along_neg_y, origin,
rotation)
np.testing.assert_allclose(answer, along_neg_y_answer, atol=1e-4)
answer = convert_global_coords_to_local(
self.along_neg_y + offset_as_list, offset, rotation)
np.testing.assert_allclose(answer, along_neg_y_answer, atol=1e-4)
def test_heading_0(self):
rotation = (1, 0, 0, 0)
origin = (0, 0, 0)
offset = (50, 25, 0)
# Testing path along pos x direction
answer = convert_global_coords_to_local(self.along_pos_x, origin,
rotation)
np.testing.assert_allclose(answer, self.along_pos_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_pos_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_x + [[50, 25]],
atol=1e-4)
# Testing path along pos y direction
answer = convert_global_coords_to_local(self.along_pos_y, origin,
rotation)
np.testing.assert_allclose(answer, self.along_neg_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_neg_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_y + [[50, 25]],
atol=1e-4)
# Testing path along neg x direction
answer = convert_global_coords_to_local(self.along_neg_x, origin,
rotation)
np.testing.assert_allclose(answer, self.along_neg_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_neg_y, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_x + [[50, 25]],
atol=1e-4)
# Testing path along neg y direction
answer = convert_global_coords_to_local(self.along_neg_y, origin,
rotation)
np.testing.assert_allclose(answer, self.along_pos_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer, self.along_pos_x, atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_y + [[50, 25]],
atol=1e-4)
def test_heading_neg_pi_over_4(self):
rotation = (np.cos(-np.pi / 8), 0, 0, np.sin(-np.pi / 8))
origin = (0, 0, 0)
offset = (50, 25, 0)
# Testing path along pos x direction
answer = convert_global_coords_to_local(self.along_pos_x, origin,
rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_x + [[50, 25]],
atol=1e-4)
# Testing path along pos y direction
answer = convert_global_coords_to_local(self.along_pos_y, origin,
rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_pos_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_pos_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[-np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_pos_y + [[50, 25]],
atol=1e-4)
# Testing path along neg x direction
answer = convert_global_coords_to_local(self.along_neg_x, origin,
rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_x,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_x + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(answer,
self.y_equals_x *
[[np.sqrt(2) / 2, -np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_x + [[50, 25]],
atol=1e-4)
# Testing path along neg y direction
answer = convert_global_coords_to_local(self.along_neg_y, origin,
rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x * [[np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, origin, rotation),
self.along_neg_y,
atol=1e-4)
answer = convert_global_coords_to_local(self.along_neg_y + [[50, 25]],
offset, rotation)
np.testing.assert_allclose(
answer,
self.y_equals_x * [[np.sqrt(2) / 2, np.sqrt(2) / 2]],
atol=1e-4)
np.testing.assert_allclose(convert_local_coords_to_global(
answer, offset, rotation),
self.along_neg_y + [[50, 25]],
atol=1e-4)