def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ possible_lanes = ray_localization(self.heading, self.spawn_place, self.engine, return_all_result=True) possible_lane_indexes = [ lane_index for lane, lane_index, dist in possible_lanes ] try: idx = possible_lane_indexes.index(self.config["spawn_lane_index"]) except ValueError: lane, new_l_index = possible_lanes[0][:-1] else: lane, new_l_index = possible_lanes[idx][:-1] dest = self.config["destination_node"] self.navigation.update( map, current_lane_index=new_l_index, final_road_node=dest if dest is not None else None, random_seed=self.engine.global_random_seed) assert lane is not None, "spawn place is not on road!" self.navigation.update_localization(self) self.lane_index = new_l_index self.lane = lane
def update_state(self, pg_world: PGWorld): lane, lane_index = ray_localization(self.position, pg_world) if lane is not None: self.vehicle_node.kinematic_model.update_lane_index( lane_index, lane) self.out_of_road = not self.vehicle_node.kinematic_model.lane.on_lane( self.vehicle_node.kinematic_model.position, margin=2)
def update_state(self, pg_world: PGWorld): dir = np.array([math.cos(self.heading), math.sin(self.heading)]) lane, lane_index = ray_localization(dir, self.position, pg_world) if lane is not None: self.vehicle_node.kinematic_model.update_lane_index( lane_index, lane) self.out_of_road = not self.vehicle_node.kinematic_model.lane.on_lane( self.vehicle_node.kinematic_model.position, margin=2)
def get_current_lane(self, ego_vehicle): possible_lanes = ray_localization(np.array( ego_vehicle.heading.tolist()), ego_vehicle.position, ego_vehicle.pg_world, return_all_result=True) for lane, index, l_1_dist in possible_lanes: if lane in self.current_ref_lanes: return lane, index return possible_lanes[0][:-1] if len(possible_lanes) > 0 else (None, None)
def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ self.routing_localization.update(map) lane, new_l_index = ray_localization((self.born_place), self.pg_world) assert lane is not None, "Born place is not on road!" self.lane_index = new_l_index self.lane = lane
def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ lane, new_l_index = ray_localization(np.array(self.heading.tolist()), np.array(self.spawn_place), self.pg_world) self.routing_localization.update(map, current_lane_index=new_l_index) assert lane is not None, "spawn place is not on road!" self.lane_index = new_l_index self.lane = lane
def get_current_lane(self, ego_vehicle): possible_lanes = ray_localization(ego_vehicle.heading, ego_vehicle.position, ego_vehicle.engine, return_all_result=True) for lane, index, l_1_dist in possible_lanes: if lane in self.current_ref_lanes: return lane, index nx_ckpt = self._target_checkpoints_index[-1] if nx_ckpt == self.checkpoints[-1] or self.next_road is None: return possible_lanes[0][:-1] if len(possible_lanes) > 0 else ( None, None) nx_nx_ckpt = nx_ckpt + 1 next_ref_lanes = self.map.road_network.graph[ self.checkpoints[nx_ckpt]][self.checkpoints[nx_nx_ckpt]] for lane, index, l_1_dist in possible_lanes: if lane in next_ref_lanes: return lane, index return possible_lanes[0][:-1] if len(possible_lanes) > 0 else (None, None)
def update_navigation_localization(self, ego_vehicle): position = ego_vehicle.position lane, lane_index = ray_localization(position, ego_vehicle.pg_world) if lane is None: lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index if self.FORCE_CALCULATE: lane_index, _ = self.map.road_network.get_closest_lane_index( position) lane = self.map.road_network.get_lane(lane_index) self._update_target_checkpoints(lane_index) target_road_1_start = self.checkpoints[ self.target_checkpoints_index[0]] target_road_1_end = self.checkpoints[self.target_checkpoints_index[0] + 1] target_road_2_start = self.checkpoints[ self.target_checkpoints_index[1]] target_road_2_end = self.checkpoints[self.target_checkpoints_index[1] + 1] target_lanes_1 = self.map.road_network.graph[target_road_1_start][ target_road_1_end] target_lanes_2 = self.map.road_network.graph[target_road_2_start][ target_road_2_end] res = [] self.current_ref_lanes = target_lanes_1 ckpts = [] lanes_heading = [] for lanes_id, lanes in enumerate([target_lanes_1, target_lanes_2]): ref_lane = lanes[0] later_middle = (float(self.map.lane_num) / 2 - 0.5) * self.map.lane_width if isinstance(ref_lane, CircularLane) and ref_lane.direction == 1: ref_lane = lanes[-1] later_middle *= -1 check_point = ref_lane.position(ref_lane.length, later_middle) if lanes_id == 0: # calculate ego v lane heading lanes_heading.append( ref_lane.heading_at( ref_lane.local_coordinates(ego_vehicle.position)[0])) else: lanes_heading.append( ref_lane.heading_at( min(self.PRE_NOTIFY_DIST, ref_lane.length))) ckpts.append(check_point) dir_vec = check_point - ego_vehicle.position dir_norm = norm(dir_vec[0], dir_vec[1]) if dir_norm > self.NAVI_POINT_DIST: dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST proj_heading, proj_side = ego_vehicle.projection(dir_vec) bendradius = 0.0 dir = 0.0 angle = 0.0 if isinstance(ref_lane, CircularLane): bendradius = ref_lane.radius / ( BlockParameterSpace.CURVE[Parameter.radius].max + self.map.lane_num * self.map.lane_width) dir = ref_lane.direction if dir == 1: angle = ref_lane.end_phase - ref_lane.start_phase elif dir == -1: angle = ref_lane.start_phase - ref_lane.end_phase res += [ clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip(bendradius, 0.0, 1.0), clip((dir + 1) / 2, 0.0, 1.0), clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0) ] if self.show_navi_point: pos_of_goal = ckpts[0] self.goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8) self.goal_node_path.setH(self.goal_node_path.getH() + 3) self.update_navi_arrow(lanes_heading) self.navi_info = res return lane, lane_index