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)