diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 7368c0254..09e0b23ca 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -317,6 +317,7 @@ jobs: pip install gymnasium==0.28 ls -al pip install -e . + pip install numpy==1.24 python -m metadrive.pull_asset cd bridges/ros_bridge diff --git a/documentation/requirements.txt b/documentation/requirements.txt index b0d853a2f..ee5f051d0 100644 --- a/documentation/requirements.txt +++ b/documentation/requirements.txt @@ -24,7 +24,7 @@ jinja2==3.1.2 # via sphinx markupsafe==2.1.1 # via jinja2 -packaging==21.3 +packaging>=22.0 # via sphinx pygments==2.12.0 # via sphinx diff --git a/metadrive/component/map/scenario_map.py b/metadrive/component/map/scenario_map.py index cac9075b1..35195ee22 100644 --- a/metadrive/component/map/scenario_map.py +++ b/metadrive/component/map/scenario_map.py @@ -12,10 +12,12 @@ class ScenarioMap(BaseMap): - def __init__(self, map_index, map_data, random_seed=None): + def __init__(self, map_index, map_data, random_seed=None, need_lane_localization=False): self.map_index = map_index self.map_data = map_data - self.need_lane_localization = self.engine.global_config.get("need_lane_localization", False) + self.need_lane_localization = need_lane_localization or self.engine.global_config.get( + "need_lane_localization", False + ) super(ScenarioMap, self).__init__(dict(id=self.map_index), random_seed=random_seed) def show_coordinates(self): diff --git a/metadrive/component/road_network/edge_road_network.py b/metadrive/component/road_network/edge_road_network.py index 172c27380..e161e1a2d 100644 --- a/metadrive/component/road_network/edge_road_network.py +++ b/metadrive/component/road_network/edge_road_network.py @@ -24,10 +24,10 @@ def add_lane(self, lane) -> None: assert lane.index is not None, "Lane index can not be None" self.graph[lane.index] = lane_info( lane=lane, - entry_lanes=lane.entry_lanes, - exit_lanes=lane.exit_lanes, - left_lanes=lane.left_lanes, - right_lanes=lane.right_lanes + entry_lanes=lane.entry_lanes or [], + exit_lanes=lane.exit_lanes or [], + left_lanes=lane.left_lanes or [], + right_lanes=lane.right_lanes or [] ) def get_lane(self, index: LaneIndex): diff --git a/metadrive/envs/legacy_envs/mixed_traffic_env.py b/metadrive/envs/legacy_envs/mixed_traffic_env.py index 17b6db6e9..61a03983e 100644 --- a/metadrive/envs/legacy_envs/mixed_traffic_env.py +++ b/metadrive/envs/legacy_envs/mixed_traffic_env.py @@ -23,7 +23,7 @@ def setup_engine(self): { "rl_agent_ratio": 0.5, "manual_control": True, - # "use_render": True, + "use_render": True, "disable_model_compression": True, # "map": "SS", "num_scenarios": 100, diff --git a/metadrive/manager/sumo_map_manager.py b/metadrive/manager/sumo_map_manager.py index 97d5cc092..b423c71f0 100644 --- a/metadrive/manager/sumo_map_manager.py +++ b/metadrive/manager/sumo_map_manager.py @@ -38,5 +38,5 @@ def reset(self): Rebuild the map and load it into the scene """ if not self.current_map: - self.current_map = ScenarioMap(map_index=0, map_data=self.map_feature) + self.current_map = ScenarioMap(map_index=0, map_data=self.map_feature, need_lane_localization=True) self.current_map.attach_to_world() diff --git a/metadrive/policy/idm_policy.py b/metadrive/policy/idm_policy.py index 8026d8472..1b9c9c791 100644 --- a/metadrive/policy/idm_policy.py +++ b/metadrive/policy/idm_policy.py @@ -459,7 +459,7 @@ def steering_control(self, target_lane) -> float: lane_heading = target_lane.heading_theta_at(long + 1) v_heading = ego_vehicle.heading_theta steering = self.heading_pid.get_result(-wrap_to_pi(lane_heading - v_heading)) - # steering += self.lateral_pid.get_result(-lat) + steering += self.lateral_pid.get_result(-lat) return float(steering) def act(self, do_speed_control, *args, **kwargs): diff --git a/metadrive/tests/vis_env/vis_sumo_map.py b/metadrive/tests/vis_env/vis_sumo_map.py index 5fa1e945e..caba831d4 100644 --- a/metadrive/tests/vis_env/vis_sumo_map.py +++ b/metadrive/tests/vis_env/vis_sumo_map.py @@ -1,10 +1,63 @@ """use netconvert --opendrive-files CARLA_town01.net.xml first""" +import logging + +import numpy as np +from metadrive.component.lane.point_lane import PointLane +from metadrive.component.vehicle.vehicle_type import SVehicle +from metadrive.engine.asset_loader import AssetLoader from metadrive.envs import BaseEnv -from metadrive.obs.observation_base import DummyObservation -import logging +from metadrive.manager.base_manager import BaseManager from metadrive.manager.sumo_map_manager import SumoMapManager -from metadrive.engine.asset_loader import AssetLoader +from metadrive.obs.observation_base import DummyObservation +from metadrive.policy.idm_policy import TrajectoryIDMPolicy +from metadrive.utils.pg.utils import ray_localization + + +class SimpleTrafficManager(BaseManager): + """ + A simple traffic creator, which creates one vehicle to follow a specified route with IDM policy. + """ + def __init__(self): + super(SimpleTrafficManager, self).__init__() + self.generated_v = None + self.arrive_dest = False + + def after_reset(self): + """ + Create vehicle and use IDM for controlling it. When there are objects in front of the vehicle, it will yield + """ + self.arrive_dest = False + path_to_follow = [] + for lane_index in ["lane_4_0", "lane_:306_0_0", "lane_22_0"]: + path_to_follow.append(self.engine.current_map.road_network.get_lane(lane_index).get_polyline()) + path_to_follow = np.concatenate(path_to_follow, axis=0) + + self.generated_v = self.spawn_object( + SVehicle, vehicle_config=dict(), position=path_to_follow[60], heading=-np.pi + ) + TrajectoryIDMPolicy.NORMAL_SPEED = 20 + self.add_policy( + self.generated_v.id, + TrajectoryIDMPolicy, + control_object=self.generated_v, + random_seed=0, + traj_to_follow=PointLane(path_to_follow, 2) + ) + + def before_step(self): + """ + When arrive destination, stop + """ + policy = self.get_policy(self.generated_v.id) + if policy.arrive_destination: + self.arrive_dest = True + + if not self.arrive_dest: + action = policy.act(do_speed_control=True) + else: + action = [0., -1] + self.generated_v.before_step(action) # set action class MyEnv(BaseEnv): @@ -29,6 +82,7 @@ def setup_engine(self): super().setup_engine() map_path = AssetLoader.file_path("carla", "CARLA_town01.net.xml", unix_style=False) self.engine.register_manager("map_manager", SumoMapManager(map_path)) + self.engine.register_manager("traffic_manager", SimpleTrafficManager()) if __name__ == "__main__": @@ -36,8 +90,7 @@ def setup_engine(self): env = MyEnv( dict( use_render=True, - # if you have a screen and OpenGL suppor, you can set use_render=True to use 3D rendering - vehicle_config={"spawn_position_heading": [(0, 0), 0]}, + vehicle_config={"spawn_position_heading": [(0, 0), np.pi / 2]}, manual_control=True, # we usually manually control the car to test environment use_mesh_terrain=True, log_level=logging.CRITICAL @@ -46,5 +99,11 @@ def setup_engine(self): env.reset() for i in range(10000): # step - obs, reward, termination, truncate, info = env.step(env.action_space.sample()) + obs, reward, termination, truncate, info, = env.step(env.action_space.sample()) + current_lane_indices = [ + info[1] for info in + ray_localization(env.vehicle.heading, env.vehicle.position, env.engine, use_heading_filter=True) + ] + + env.render(text={"current_lane_indices": current_lane_indices}) env.close() diff --git a/metadrive/utils/sumo/map_utils.py b/metadrive/utils/sumo/map_utils.py index 2d88c7d3d..fcee0abf7 100644 --- a/metadrive/utils/sumo/map_utils.py +++ b/metadrive/utils/sumo/map_utils.py @@ -306,17 +306,17 @@ def extract_map_features(graph): # # build map boundary polygons = [] - for junction_id, junction in graph.junctions.items(): - if len(junction.shape) <= 2: - continue - boundary_polygon = Polygon(junction.shape) - boundary_polygon = [(x, y) for x, y in boundary_polygon.exterior.coords] - id = "junction_{}".format(junction.name) - ret[id] = { - SD.TYPE: MetaDriveType.LANE_SURFACE_STREET, - SD.POLYLINE: junction.shape, - SD.POLYGON: boundary_polygon, - } + # for junction_id, junction in graph.junctions.items(): + # if len(junction.shape) <= 2: + # continue + # boundary_polygon = Polygon(junction.shape) + # boundary_polygon = [(x, y) for x, y in boundary_polygon.exterior.coords] + # id = "junction_{}".format(junction.name) + # ret[id] = { + # SD.TYPE: MetaDriveType.LANE_SURFACE_STREET, + # SD.POLYLINE: junction.shape, + # SD.POLYGON: boundary_polygon, + # } # build map lanes for road_id, road in graph.roads.items():