| 470 | |
| 471 | |
| 472 | def get_target_gps(self, gps, compass): |
| 473 | # target gps |
| 474 | def gps_to_location(gps): |
| 475 | lat, lon, z = gps |
| 476 | lat = float(lat) |
| 477 | lon = float(lon) |
| 478 | z = float(z) |
| 479 | |
| 480 | location = carla.Location(z=z) |
| 481 | xy = (gps[:2] - self._command_planner.mean) * self._command_planner.scale |
| 482 | location.x = xy[0] |
| 483 | location.y = -xy[1] |
| 484 | return location |
| 485 | global_plan_gps = self._global_plan |
| 486 | next_gps, _ = global_plan_gps[self.navigation_idx+1] |
| 487 | next_gps = np.array([next_gps['lat'], next_gps['lon'], next_gps['z']]) |
| 488 | next_vec_in_global = gps_to_location(next_gps) - gps_to_location(gps) |
| 489 | ref_rot_in_global = carla.Rotation(yaw=np.rad2deg(compass)-90.0) |
| 490 | loc_in_ev = trans_utils.vec_global_to_ref(next_vec_in_global, ref_rot_in_global) |
| 491 | |
| 492 | if np.sqrt(loc_in_ev.x**2+loc_in_ev.y**2) < 12.0 and loc_in_ev.x < 0.0: |
| 493 | self.navigation_idx += 1 |
| 494 | |
| 495 | self.navigation_idx = min(self.navigation_idx, len(global_plan_gps)-2) |
| 496 | |
| 497 | _, road_option_0 = global_plan_gps[max(0, self.navigation_idx)] |
| 498 | gps_point, road_option_1 = global_plan_gps[self.navigation_idx+1] |
| 499 | gps_point = np.array([gps_point['lat'], gps_point['lon'], gps_point['z']]) |
| 500 | |
| 501 | if (road_option_0 in [RoadOption.CHANGELANELEFT, RoadOption.CHANGELANERIGHT]) \ |
| 502 | and (road_option_1 not in [RoadOption.CHANGELANELEFT, RoadOption.CHANGELANERIGHT]): |
| 503 | road_option = road_option_1 |
| 504 | else: |
| 505 | road_option = road_option_0 |
| 506 | |
| 507 | return np.array(gps_point, dtype=np.float32), np.array([road_option.value], dtype=np.int8) |
| 508 | |
| 509 | |
| 510 | def process_act(self, action): |