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