diff --git a/metadrive/component/navigation_module/trajectory_navigation.py b/metadrive/component/navigation_module/trajectory_navigation.py index 308c159ae..e36482391 100644 --- a/metadrive/component/navigation_module/trajectory_navigation.py +++ b/metadrive/component/navigation_module/trajectory_navigation.py @@ -33,9 +33,9 @@ def __init__( if show_dest_mark or show_line_to_dest: get_logger().warning("show_dest_mark and show_line_to_dest are not supported in TrajectoryNavigation") super(TrajectoryNavigation, self).__init__( - show_navi_mark=False, - show_dest_mark=False, - show_line_to_dest=False, + show_navi_mark=show_navi_mark, + show_dest_mark=show_dest_mark, + show_line_to_dest=show_line_to_dest, panda_color=panda_color, name=name, vehicle_config=vehicle_config @@ -145,6 +145,17 @@ def update_localization(self, ego_vehicle): # Use RC as the only criterion to determine arrival in Scenario env. self._route_completion = long / self.reference_trajectory.length + if self._show_navi_info: + # Whether to visualize little boxes in the scene denoting the checkpoints + pos_of_goal = ckpts[1] + self._goal_node_path.setPos(panda_vector(pos_of_goal[0], pos_of_goal[1], self.MARK_HEIGHT)) + self._goal_node_path.setH(self._goal_node_path.getH() + 3) + # self.navi_arrow_dir = [lanes_heading1, lanes_heading2] + dest_pos = self._dest_node_path.getPos() + self._draw_line_to_dest(start_position=ego_vehicle.position, end_position=(dest_pos[0], dest_pos[1])) + navi_pos = self._goal_node_path.getPos() + self._draw_line_to_navi(start_position=ego_vehicle.position, end_position=(navi_pos[0], navi_pos[1])) + def get_current_lateral_range(self, current_position, engine) -> float: return self.current_lane.width * 2 diff --git a/metadrive/component/sensors/semantic_camera.py b/metadrive/component/sensors/semantic_camera.py index de4a4f0b7..480cbb5c9 100644 --- a/metadrive/component/sensors/semantic_camera.py +++ b/metadrive/component/sensors/semantic_camera.py @@ -38,7 +38,7 @@ def _setup_effect(self): ) else: - if label == Semantics.PEDESTRIAN.label: + if label == Semantics.PEDESTRIAN.label and not self.engine.global_config.get("use_bounding_box", False): # PZH: This is a workaround fix to make pedestrians animated. cam.setTagState( label, diff --git a/metadrive/component/traffic_participants/base_traffic_participant.py b/metadrive/component/traffic_participants/base_traffic_participant.py index 6406ec03f..6f2669e9a 100644 --- a/metadrive/component/traffic_participants/base_traffic_participant.py +++ b/metadrive/component/traffic_participants/base_traffic_participant.py @@ -14,8 +14,8 @@ class BaseTrafficParticipant(BaseObject): COLLISION_MASK = CollisionGroup.TrafficParticipants HEIGHT = None - def __init__(self, position: Sequence[float], heading_theta: float = 0., random_seed=None, name=None): - super(BaseTrafficParticipant, self).__init__(random_seed=random_seed, name=name) + def __init__(self, position: Sequence[float], heading_theta: float = 0., random_seed=None, name=None, config=None): + super(BaseTrafficParticipant, self).__init__(random_seed=random_seed, name=name, config=config) self.set_position(position, self.HEIGHT / 2 if hasattr(self, "HEIGHT") else 0) self.set_heading_theta(heading_theta) assert self.MASS is not None, "No mass for {}".format(self.class_name) diff --git a/metadrive/component/traffic_participants/cyclist.py b/metadrive/component/traffic_participants/cyclist.py index 036271a8e..f5440d6cd 100644 --- a/metadrive/component/traffic_participants/cyclist.py +++ b/metadrive/component/traffic_participants/cyclist.py @@ -8,7 +8,8 @@ from metadrive.constants import CollisionGroup from metadrive.engine.asset_loader import AssetLoader from metadrive.engine.physics_node import BaseRigidBodyNode - +from panda3d.core import TransparencyAttrib, LineSegs, NodePath, BoundingHexahedron +from panda3d.core import Material, Vec3, LVecBase4 class Cyclist(BaseTrafficParticipant): MASS = 80 # kg @@ -19,13 +20,13 @@ class Cyclist(BaseTrafficParticipant): HEIGHT = 1.75 - def __init__(self, position, heading_theta, random_seed, name=None): + def __init__(self, position, heading_theta, random_seed, name=None, *args, **kwargs): super(Cyclist, self).__init__(position, heading_theta, random_seed, name=name) self.set_metadrive_type(self.TYPE_NAME) n = BaseRigidBodyNode(self.name, self.TYPE_NAME) self.add_body(n) - self.body.addShape(BulletBoxShape((self.LENGTH / 2, self.WIDTH / 2, self.HEIGHT / 2))) + self.body.addShape(BulletBoxShape((self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2))) if self.render: if Cyclist.MODEL is None: model = self.loader.loadModel(AssetLoader.file_path("models", "bicycle", "scene.gltf")) @@ -45,3 +46,161 @@ def WIDTH(self): @property def LENGTH(self): return 1.75 + + +class CyclistBoundingBox(BaseTrafficParticipant): + MASS = 80 # kg + TYPE_NAME = MetaDriveType.CYCLIST + COLLISION_MASK = CollisionGroup.TrafficParticipants + SEMANTIC_LABEL = Semantics.BIKE.label + + # for random color choosing + MATERIAL_COLOR_COEFF = 1.6 # to resist other factors, since other setting may make color dark + MATERIAL_METAL_COEFF = 0.1 # 0-1 + MATERIAL_ROUGHNESS = 0.8 # smaller to make it more smooth, and reflect more light + MATERIAL_SHININESS = 128 # 0-128 smaller to make it more smooth, and reflect more light + MATERIAL_SPECULAR_COLOR = (3, 3, 3, 3) + + def __init__(self, position, heading_theta, random_seed, name=None, **kwargs): + config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]} + # config = {"width": kwargs["length"], "length": kwargs["width"], "height": kwargs["height"]} + super(CyclistBoundingBox, self).__init__(position, heading_theta, random_seed, name=name, config=config) + self.set_metadrive_type(self.TYPE_NAME) + n = BaseRigidBodyNode(self.name, self.TYPE_NAME) + self.add_body(n) + + self.body.addShape(BulletBoxShape((self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2))) + if self.render: + model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) + model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) + model.setTwoSided(False) + self._instance = model.instanceTo(self.origin) + + # Add some color to help debug + from panda3d.core import Material, LVecBase4 + import seaborn as sns + + show_contour = self.config["show_contour"] if "show_contour" in self.config else False + if show_contour: + # ========== Draw the contour of the bounding box ========== + # Draw the bottom of the car first + line_seg = LineSegs("bounding_box_contour1") + zoffset = model.getZ() + line_seg.setThickness(2) + line_color = [0.0, 0.0, 0.0] + out_offset = 0.02 + w = self.WIDTH / 2 + out_offset + l = self.LENGTH / 2 + out_offset + h = self.HEIGHT / 2 + out_offset + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(-w, l, h + zoffset) + line_seg.drawTo(-w, l, -h + zoffset) + line_seg.drawTo(w, l, -h + zoffset) + line_seg.drawTo(w, l, h + zoffset) + + # draw cross line + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_seg.moveTo(w, -l, h + zoffset) + line_seg.drawTo(w, l, -h + zoffset) + + line_seg.moveTo(w, -l, h + zoffset) + line_seg.drawTo(-w, -l, h + zoffset) + line_seg.drawTo(-w, -l, -h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_seg.drawTo(w, -l, h + zoffset) + + # draw vertical & horizontal line + line_seg.moveTo(-w, l, 0 + zoffset) + line_seg.drawTo(-w, -l, 0 + zoffset) + line_seg.moveTo(-w, 0, h + zoffset) + line_seg.drawTo(-w, 0, -h + zoffset) + + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(w, -l, h + zoffset) + line_seg.moveTo(-w, l, h + zoffset) + line_seg.drawTo(-w, -l, h + zoffset) + line_seg.moveTo(-w, l, -h + zoffset) + line_seg.drawTo(-w, -l, -h + zoffset) + line_seg.moveTo(w, l, -h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_np = NodePath(line_seg.create(True)) + line_material = Material() + line_material.setBaseColor(LVecBase4(*line_color[:3], 1)) + line_np.setMaterial(line_material, True) + line_np.reparentTo(self.origin) + + color = sns.color_palette("colorblind") + color.remove(color[2]) # Remove the green and leave it for special vehicle + idx = 0 + rand_c = color[idx] + rand_c = (1.0, 0.0, 0.0) + self._panda_color = rand_c + material = Material() + material.setBaseColor( + ( + self.panda_color[0] * self.MATERIAL_COLOR_COEFF, self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0. + ) + ) + material.setMetallic(self.MATERIAL_METAL_COEFF) + material.setSpecular(self.MATERIAL_SPECULAR_COLOR) + material.setRefractiveIndex(1.5) + material.setRoughness(self.MATERIAL_ROUGHNESS) + material.setShininess(self.MATERIAL_SHININESS) + material.setTwoside(False) + self.origin.setMaterial(material, True) + + def reset(self, position, heading_theta: float = 0., random_seed=None, name=None, *args, **kwargs): + super(CyclistBoundingBox, self).reset(position, heading_theta, random_seed, name, *args, **kwargs) + config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]} + self.update_config(config) + if self._instance is not None: + self._instance.detachNode() + if self.render: + model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) + model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) + model.setTwoSided(False) + self._instance = model.instanceTo(self.origin) + + # Add some color to help debug + from panda3d.core import Material, LVecBase4 + import seaborn as sns + color = sns.color_palette("colorblind") + color.remove(color[2]) # Remove the green and leave it for special vehicle + idx = 0 + rand_c = color[idx] + rand_c = (1.0, 0.0, 0.0) + self._panda_color = rand_c + material = Material() + material.setBaseColor( + ( + self.panda_color[0] * self.MATERIAL_COLOR_COEFF, self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0. + ) + ) + material.setMetallic(self.MATERIAL_METAL_COEFF) + material.setSpecular(self.MATERIAL_SPECULAR_COLOR) + material.setRefractiveIndex(1.5) + material.setRoughness(self.MATERIAL_ROUGHNESS) + material.setShininess(self.MATERIAL_SHININESS) + material.setTwoside(False) + self.origin.setMaterial(material, True) + + def set_velocity(self, direction, value=None, in_local_frame=False): + super(CyclistBoundingBox, self).set_velocity(direction, value, in_local_frame) + self.standup() + + @property + def WIDTH(self): + # return self.config["width"] + return self.config["length"] + + @property + def HEIGHT(self): + return self.config["height"] + + @property + def LENGTH(self): + # return self.config["length"] + return self.config["width"] diff --git a/metadrive/component/traffic_participants/pedestrian.py b/metadrive/component/traffic_participants/pedestrian.py index fed51f295..5955f57e2 100644 --- a/metadrive/component/traffic_participants/pedestrian.py +++ b/metadrive/component/traffic_participants/pedestrian.py @@ -1,5 +1,5 @@ from direct.actor.Actor import Actor -from panda3d.bullet import BulletCylinderShape +from panda3d.bullet import BulletCylinderShape, BulletBoxShape from panda3d.core import LVector3 from metadrive.component.traffic_participants.base_traffic_participant import BaseTrafficParticipant @@ -7,7 +7,8 @@ from metadrive.engine.asset_loader import AssetLoader from metadrive.engine.physics_node import BaseRigidBodyNode from metadrive.utils.math import norm - +from panda3d.core import TransparencyAttrib, LineSegs, NodePath, BoundingHexahedron +from panda3d.core import Material, Vec3, LVecBase4 class Pedestrian(BaseTrafficParticipant): MASS = 70 # kg @@ -21,7 +22,7 @@ class Pedestrian(BaseTrafficParticipant): # SPEED_LIST = [0.6, 1.2, 2.2] Too much speed choice jeopardise the performance SPEED_LIST = [0.4, 1.2] - def __init__(self, position, heading_theta, random_seed=None, name=None): + def __init__(self, position, heading_theta, random_seed=None, name=None, *args, **kwargs): super(Pedestrian, self).__init__(position, heading_theta, random_seed, name=name) self.set_metadrive_type(self.TYPE_NAME) # self.origin.setDepthOffset(1) @@ -116,3 +117,160 @@ def top_down_width(self): @property def top_down_length(self): return self.RADIUS * 2 + + +class PedestrainBoundingBox(BaseTrafficParticipant): + MASS = 70 # kg + TYPE_NAME = MetaDriveType.PEDESTRIAN + SEMANTIC_LABEL = Semantics.PEDESTRIAN.label + + # for random color choosing + MATERIAL_COLOR_COEFF = 1.6 # to resist other factors, since other setting may make color dark + MATERIAL_METAL_COEFF = 0.1 # 0-1 + MATERIAL_ROUGHNESS = 0.8 # smaller to make it more smooth, and reflect more light + MATERIAL_SHININESS = 128 # 0-128 smaller to make it more smooth, and reflect more light + MATERIAL_SPECULAR_COLOR = (3, 3, 3, 3) + + def __init__(self, position, heading_theta, width, length, height, random_seed=None, name=None): + config = {} + config["width"] = width + config["length"] = length + config["height"] = height + + super(PedestrainBoundingBox, self).__init__(position, heading_theta, random_seed, name=name, config=config) + self.set_metadrive_type(self.TYPE_NAME) + n = BaseRigidBodyNode(self.name, self.TYPE_NAME) + self.add_body(n) + + # PZH: Use BoxShape instead of CylinderShape + # self.body.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT)) + self.body.addShape(BulletBoxShape(LVector3(width / 2, length / 2, height / 2))) + + self._instance = None + if self.render: + # PZH: Load a box model and resize it to the vehicle size + model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) + model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) + model.setTwoSided(False) + self._instance = model.instanceTo(self.origin) + + # Add some color to help debug + from panda3d.core import Material + import seaborn as sns + + show_contour = self.config["show_contour"] if "show_contour" in self.config else False + if show_contour: + # ========== Draw the contour of the bounding box ========== + # Draw the bottom of the car first + line_seg = LineSegs("bounding_box_contour1") + zoffset = model.getZ() + line_seg.setThickness(2) + line_color = [0.0, 0.0, 1.0] + out_offset = 0.02 + w = self.WIDTH / 2 + out_offset + l = self.LENGTH / 2 + out_offset + h = self.HEIGHT / 2 + out_offset + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(-w, l, h + zoffset) + line_seg.drawTo(-w, l, -h + zoffset) + line_seg.drawTo(w, l, -h + zoffset) + line_seg.drawTo(w, l, h + zoffset) + + # draw cross line + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_seg.moveTo(w, -l, h + zoffset) + line_seg.drawTo(w, l, -h + zoffset) + + line_seg.moveTo(w, -l, h + zoffset) + line_seg.drawTo(-w, -l, h + zoffset) + line_seg.drawTo(-w, -l, -h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_seg.drawTo(w, -l, h + zoffset) + + # draw vertical & horizontal line + line_seg.moveTo(-w, l, 0 + zoffset) + line_seg.drawTo(-w, -l, 0 + zoffset) + line_seg.moveTo(-w, 0, h + zoffset) + line_seg.drawTo(-w, 0, -h + zoffset) + + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(w, -l, h + zoffset) + line_seg.moveTo(-w, l, h + zoffset) + line_seg.drawTo(-w, -l, h + zoffset) + line_seg.moveTo(-w, l, -h + zoffset) + line_seg.drawTo(-w, -l, -h + zoffset) + line_seg.moveTo(w, l, -h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_np = NodePath(line_seg.create(True)) + line_material = Material() + line_material.setBaseColor(LVecBase4(*line_color[:3], 1)) + line_np.setMaterial(line_material, True) + line_np.reparentTo(self.origin) + + color = sns.color_palette("colorblind") + color.remove(color[2]) # Remove the green and leave it for special vehicle + idx = 0 + rand_c = color[idx] + rand_c = (0.0, 1.0, 0.0) + self._panda_color = rand_c + material = Material() + material.setBaseColor( + ( + self.panda_color[0] * self.MATERIAL_COLOR_COEFF, self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0. + ) + ) + material.setMetallic(self.MATERIAL_METAL_COEFF) + material.setSpecular(self.MATERIAL_SPECULAR_COLOR) + material.setRefractiveIndex(1.5) + material.setRoughness(self.MATERIAL_ROUGHNESS) + material.setShininess(self.MATERIAL_SHININESS) + material.setTwoside(False) + self.origin.setMaterial(material, True) + + def reset(self, position, heading_theta: float = 0., random_seed=None, name=None, *args, **kwargs): + super(PedestrainBoundingBox, self).reset(position, heading_theta, random_seed, name, *args, **kwargs) + config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]} + self.update_config(config) + if self._instance is not None: + self._instance.detachNode() + if self.render: + model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) + model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) + model.setTwoSided(False) + self._instance = model.instanceTo(self.origin) + + def set_velocity(self, direction: list, value=None, in_local_frame=False): + self.set_roll(0) + self.set_pitch(0) + if in_local_frame: + from metadrive.engine.engine_utils import get_engine + engine = get_engine() + direction = LVector3(*direction, 0.) + direction[1] *= -1 + ret = engine.worldNP.getRelativeVector(self.origin, direction) + direction = ret + speed = (norm(direction[0], direction[1]) + 1e-6) + if value is not None: + norm_ratio = value / speed + else: + norm_ratio = 1 + + self._body.setLinearVelocity( + LVector3(direction[0] * norm_ratio, direction[1] * norm_ratio, + self._body.getLinearVelocity()[-1]) + ) + self.standup() + + @property + def HEIGHT(self): + return self.config["height"] + + @property + def LENGTH(self): + return self.config["length"] + + @property + def WIDTH(self): + return self.config["width"] diff --git a/metadrive/component/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index 98eab0a08..d4de1189c 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -1,8 +1,12 @@ import platform -from metadrive.component.pg_space import ParameterSpace, VehicleParameterSpace +from panda3d.core import Material, Vec3, LVecBase4 + +from metadrive.component.pg_space import VehicleParameterSpace, ParameterSpace from metadrive.component.vehicle.base_vehicle import BaseVehicle from metadrive.constants import Semantics +from metadrive.engine.asset_loader import AssetLoader +from panda3d.core import TransparencyAttrib, LineSegs, NodePath, BoundingHexahedron class DefaultVehicle(BaseVehicle): @@ -208,25 +212,25 @@ def reset( if vehicle_config["length"] is not None and vehicle_config["length"] != self.LENGTH: should_force_reset = True if "max_engine_force" in vehicle_config and \ - vehicle_config["max_engine_force"] is not None and \ - vehicle_config["max_engine_force"] != self.config["max_engine_force"]: + vehicle_config["max_engine_force"] is not None and \ + vehicle_config["max_engine_force"] != self.config["max_engine_force"]: should_force_reset = True if "max_brake_force" in vehicle_config and \ - vehicle_config["max_brake_force"] is not None and \ - vehicle_config["max_brake_force"] != self.config["max_brake_force"]: + vehicle_config["max_brake_force"] is not None and \ + vehicle_config["max_brake_force"] != self.config["max_brake_force"]: should_force_reset = True if "wheel_friction" in vehicle_config and \ - vehicle_config["wheel_friction"] is not None and \ - vehicle_config["wheel_friction"] != self.config["wheel_friction"]: + vehicle_config["wheel_friction"] is not None and \ + vehicle_config["wheel_friction"] != self.config["wheel_friction"]: should_force_reset = True if "max_steering" in vehicle_config and \ - vehicle_config["max_steering"] is not None and \ - vehicle_config["max_steering"] != self.config["max_steering"]: + vehicle_config["max_steering"] is not None and \ + vehicle_config["max_steering"] != self.config["max_steering"]: self.max_steering = vehicle_config["max_steering"] should_force_reset = True if "mass" in vehicle_config and \ - vehicle_config["mass"] is not None and \ - vehicle_config["mass"] != self.config["mass"]: + vehicle_config["mass"] is not None and \ + vehicle_config["mass"] != self.config["mass"]: should_force_reset = True # def process_memory(): @@ -266,6 +270,146 @@ def reset( return ret +class VaryingDynamicsBoundingBoxVehicle(VaryingDynamicsVehicle): + + # def after_step(self): + # r = super(VaryingDynamicsBoundingBoxVehicle, self).after_step() + # + # line_seg = self._line_seg + # line_color = (1.0, 1.0, 1.0) + # line_seg.setColor(line_color[0], line_color[1], line_color[2], 1.0) + # line_seg.setThickness(10) + # line_offset = 0 + # # Draw the bottom of the car first + # line_seg.moveTo(-self.WIDTH / 2 - line_offset, -self.LENGTH / 2 - line_offset, -self.HEIGHT / 2 - line_offset) + # line_seg.drawTo(self.WIDTH / 2 + line_offset, -self.LENGTH / 2 - line_offset, -self.HEIGHT / 2 - line_offset) + # line_seg.drawTo(self.WIDTH / 2 + line_offset, -self.LENGTH / 2 - line_offset, self.HEIGHT / 2 + line_offset) + # line_seg.drawTo(-self.WIDTH / 2 - line_offset, -self.LENGTH / 2 - line_offset, self.HEIGHT / 2 + line_offset) + # + # self._line_seg_node.removeNode() + # self._node_path_list.remove(self._line_seg_node) + # self._line_seg_node = NodePath(line_seg.create(True)) + # self._node_path_list.append(self._line_seg_node) + # self._line_seg_node.reparentTo(self.origin) + # + # + # + # + # + # + # return r + + def _add_visualization(self): + if self.render: + [path, scale, offset, HPR] = self.path + + # PZH: Note that we do not use model_collection as a buffer here. + # if path not in BaseVehicle.model_collection: + + # PZH: Load a box model and resize it to the vehicle size + car_model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) + + car_model.setTwoSided(False) + BaseVehicle.model_collection[path] = car_model + car_model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) + # car_model.setZ(-self.TIRE_RADIUS - self.CHASSIS_TO_WHEEL_AXIS + self.HEIGHT / 2) + car_model.setZ(0) + # model default, face to y + car_model.setHpr(*HPR) + car_model.instanceTo(self.origin) + + show_contour = self.config["show_contour"] if "show_contour" in self.config else False + if show_contour: + # ========== Draw the contour of the bounding box ========== + # Draw the bottom of the car first + line_seg = LineSegs("bounding_box_contour1") + zoffset = car_model.getZ() + line_seg.setThickness(2) + line_color = [1.0, 0.0, 0.0] + out_offset = 0.02 + w = self.WIDTH / 2 + out_offset + l = self.LENGTH / 2 + out_offset + h = self.HEIGHT / 2 + out_offset + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(-w, l, h + zoffset) + line_seg.drawTo(-w, l, -h + zoffset) + line_seg.drawTo(w, l, -h + zoffset) + line_seg.drawTo(w, l, h + zoffset) + line_seg.drawTo(-w, l, -h + zoffset) + line_seg.moveTo(-w, l, h + zoffset) + line_seg.drawTo(w, l, -h + zoffset) + + line_seg.moveTo(w, -l, h + zoffset) + line_seg.drawTo(-w, -l, h + zoffset) + line_seg.drawTo(-w, -l, -h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_seg.drawTo(w, -l, h + zoffset) + line_seg.moveTo(-w, -l, 0 + zoffset) + line_seg.drawTo(w, -l, 0 + zoffset) + line_seg.moveTo(0, -l, h + zoffset) + line_seg.drawTo(0, -l, -h + zoffset) + + line_seg.moveTo(w, l, h + zoffset) + line_seg.drawTo(w, -l, h + zoffset) + line_seg.moveTo(-w, l, h + zoffset) + line_seg.drawTo(-w, -l, h + zoffset) + line_seg.moveTo(-w, l, -h + zoffset) + line_seg.drawTo(-w, -l, -h + zoffset) + line_seg.moveTo(w, l, -h + zoffset) + line_seg.drawTo(w, -l, -h + zoffset) + line_np = NodePath(line_seg.create(True)) + line_material = Material() + line_material.setBaseColor(LVecBase4(*line_color[:3], 1)) + line_np.setMaterial(line_material, True) + line_np.reparentTo(self.origin) + + if self.config["random_color"]: + material = Material() + material.setBaseColor( + ( + self.panda_color[0] * self.MATERIAL_COLOR_COEFF, + self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0. + ) + ) + material.setMetallic(self.MATERIAL_METAL_COEFF) + material.setSpecular(self.MATERIAL_SPECULAR_COLOR) + material.setRefractiveIndex(1.5) + material.setRoughness(self.MATERIAL_ROUGHNESS) + material.setShininess(self.MATERIAL_SHININESS) + material.setTwoside(False) + self.origin.setMaterial(material, True) + + def _add_wheel(self, pos: Vec3, radius: float, front: bool, left): + wheel_np = self.origin.attachNewNode("wheel") + self._node_path_list.append(wheel_np) + + # PZH: Skip the wheel model + # if self.render: + # model = 'right_tire_front.gltf' if front else 'right_tire_back.gltf' + # model_path = AssetLoader.file_path("models", os.path.dirname(self.path[0]), model) + # wheel_model = self.loader.loadModel(model_path) + # wheel_model.setTwoSided(self.TIRE_TWO_SIDED) + # wheel_model.reparentTo(wheel_np) + # wheel_model.set_scale(1 * self.TIRE_MODEL_CORRECT if left else -1 * self.TIRE_MODEL_CORRECT) + wheel = self.system.createWheel() + wheel.setNode(wheel_np.node()) + wheel.setChassisConnectionPointCs(pos) + wheel.setFrontWheel(front) + wheel.setWheelDirectionCs(Vec3(0, 0, -1)) + wheel.setWheelAxleCs(Vec3(1, 0, 0)) + + wheel.setWheelRadius(radius) + wheel.setMaxSuspensionTravelCm(self.SUSPENSION_LENGTH) + wheel.setSuspensionStiffness(self.SUSPENSION_STIFFNESS) + wheel.setWheelsDampingRelaxation(4.8) + wheel.setWheelsDampingCompression(1.2) + wheel_friction = self.config["wheel_friction"] if not self.config["no_wheel_friction"] else 0 + wheel.setFrictionSlip(wheel_friction) + wheel.setRollInfluence(0.5) + return wheel + + def random_vehicle_type(np_random, p=None): v_type = { "s": SVehicle, @@ -289,6 +433,7 @@ def random_vehicle_type(np_random, p=None): "default": DefaultVehicle, "static_default": StaticDefaultVehicle, "varying_dynamics": VaryingDynamicsVehicle, + "varying_dynamics_bounding_box": VaryingDynamicsBoundingBoxVehicle, "traffic_default": TrafficDefaultVehicle } diff --git a/metadrive/constants.py b/metadrive/constants.py index 00f5cc5bd..8b54544dd 100644 --- a/metadrive/constants.py +++ b/metadrive/constants.py @@ -232,11 +232,13 @@ def collision_rules(cls): ] @classmethod - def set_collision_rule(cls, world: BulletWorld): + def set_collision_rule(cls, world: BulletWorld, disable_collision: bool = False): for rule in cls.collision_rules(): group_1 = int(math.log(rule[0].getWord(), 2)) group_2 = int(math.log(rule[1].getWord(), 2)) relation = rule[-1] + if disable_collision: + relation = False world.setGroupCollisionFlag(group_1, group_2, relation) @classmethod diff --git a/metadrive/engine/core/engine_core.py b/metadrive/engine/core/engine_core.py index 4ba39f498..b6f277fb9 100644 --- a/metadrive/engine/core/engine_core.py +++ b/metadrive/engine/core/engine_core.py @@ -259,7 +259,9 @@ def __init__(self, global_config): self.common_filter = None # physics world - self.physics_world = PhysicsWorld(self.global_config["debug_static_world"]) + self.physics_world = PhysicsWorld( + self.global_config["debug_static_world"], disable_collision=self.global_config["disable_collision"] + ) # collision callback self.physics_world.dynamic_world.setContactAddedCallback(PythonCallbackObject(collision_callback)) diff --git a/metadrive/engine/core/physics_world.py b/metadrive/engine/core/physics_world.py index 933075c25..bfb8d529a 100644 --- a/metadrive/engine/core/physics_world.py +++ b/metadrive/engine/core/physics_world.py @@ -7,14 +7,14 @@ class PhysicsWorld: - def __init__(self, debug=False): + def __init__(self, debug=False, disable_collision=False): # a dynamic world, moving objects or objects which react to other objects should be placed here self.dynamic_world = BulletWorld() - CollisionGroup.set_collision_rule(self.dynamic_world) + CollisionGroup.set_collision_rule(self.dynamic_world, disable_collision=disable_collision) self.dynamic_world.setGravity(Vec3(0, 0, -9.81)) # set gravity # a static world which used to query position/overlap .etc. Don't implement doPhysics() in this world self.static_world = BulletWorld() if not debug else self.dynamic_world - CollisionGroup.set_collision_rule(self.static_world) + CollisionGroup.set_collision_rule(self.static_world, disable_collision=disable_collision) def report_bodies(self): dynamic_bodies = \ diff --git a/metadrive/envs/base_env.py b/metadrive/envs/base_env.py index 8846238d9..bebe1e46f 100644 --- a/metadrive/envs/base_env.py +++ b/metadrive/envs/base_env.py @@ -209,6 +209,8 @@ preload_models=True, # model compression increasing the launch time disable_model_compression=True, + # Whether to disable the collision detection (useful for debugging / replay logged scenarios) + disable_collision=False, # ===== Terrain ===== # The size of the square map region, which is centered at [0, 0]. The map objects outside it are culled. diff --git a/metadrive/envs/scenario_env.py b/metadrive/envs/scenario_env.py index 7831742be..475d91fca 100644 --- a/metadrive/envs/scenario_env.py +++ b/metadrive/envs/scenario_env.py @@ -3,12 +3,12 @@ """ import numpy as np -from metadrive.component.navigation_module.edge_network_navigation import EdgeNetworkNavigation + from metadrive.component.navigation_module.trajectory_navigation import TrajectoryNavigation -from metadrive.constants import DEFAULT_AGENT from metadrive.constants import TerminationState from metadrive.engine.asset_loader import AssetLoader from metadrive.envs.base_env import BaseEnv +from metadrive.manager.scenario_agent_manager import ScenarioAgentManager from metadrive.manager.scenario_curriculum_manager import ScenarioCurriculumManager from metadrive.manager.scenario_data_manager import ScenarioDataManager from metadrive.manager.scenario_light_manager import ScenarioLightManager @@ -90,7 +90,8 @@ # ===== others ===== allowed_more_steps=None, # horizon, None=infinite - top_down_show_real_size=False + top_down_show_real_size=False, + use_bounding_box=False, # Set True to use a cube in visualization to represent every dynamic objects. ) @@ -115,6 +116,18 @@ def __init__(self, config=None): self.start_index = self.config["start_scenario_index"] self.num_scenarios = self.config["num_scenarios"] + def _post_process_config(self, config): + config = super(ScenarioEnv, self)._post_process_config(config) + if config["use_bounding_box"]: + config["vehicle_config"]["random_color"] = True + config["vehicle_config"]["vehicle_model"] = "varying_dynamics_bounding_box" + config["agent_configs"]["default_agent"]["use_special_color"] = True + config["agent_configs"]["default_agent"]["vehicle_model"] = "varying_dynamics_bounding_box" + return config + + def _get_agent_manager(self): + return ScenarioAgentManager(init_observations=self._get_observations()) + def setup_engine(self): super(ScenarioEnv, self).setup_engine() self.engine.register_manager("data_manager", ScenarioDataManager()) @@ -179,7 +192,7 @@ def msg(reason): done = True self.logger.info(msg("max step"), extra={"log_once": True}) elif self.config["allowed_more_steps"] and self.episode_lengths[vehicle_id] >= \ - self.engine.data_manager.current_scenario_length + self.config["allowed_more_steps"]: + self.engine.data_manager.current_scenario_length + self.config["allowed_more_steps"]: if self.config["truncate_as_terminate"]: done = True done_info[TerminationState.MAX_STEP] = True diff --git a/metadrive/examples/drive_in_real_env_with_bounding_box.py b/metadrive/examples/drive_in_real_env_with_bounding_box.py new file mode 100755 index 000000000..b26142a0a --- /dev/null +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +""" +This script demonstrates how to use the environment where traffic and road map are loaded from Waymo dataset. +""" +import argparse +import random + +from metadrive.constants import HELP_MESSAGE +from metadrive.engine.asset_loader import AssetLoader +from metadrive.envs.scenario_env import ScenarioEnv + +RENDER_MESSAGE = { + "Quit": "ESC", + "Switch perspective": "Q or B", + "Reset Episode": "R", + "Keyboard Control": "W,A,S,D", +} + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--reactive_traffic", action="store_true") + parser.add_argument("--top_down", "--topdown", action="store_true") + parser.add_argument("--waymo", action="store_true") + args = parser.parse_args() + extra_args = dict(film_size=(2000, 2000)) if args.top_down else {} + asset_path = AssetLoader.asset_path + use_waymo = args.waymo + print(HELP_MESSAGE) + try: + env = ScenarioEnv( + { + "manual_control": True, + "sequential_seed": True, + "reactive_traffic": True if args.reactive_traffic else False, + "use_render": True if not args.top_down else False, + "data_directory": AssetLoader.file_path( + asset_path, "waymo" if use_waymo else "nuscenes", unix_style=False + ), + "num_scenarios": 3 if use_waymo else 10, + "debug": True, + "use_bounding_box": True, + "vehicle_config": { + "show_line_to_dest": True, + "show_line_to_navi_mark": True, + }, + "disable_collision": True, + } + ) + o, _ = env.reset() + + for i in range(1, 100000): + o, r, tm, tc, info = env.step([1.0, 0.]) + env.render( + mode="top_down" if args.top_down else None, + text=None if args.top_down else RENDER_MESSAGE, + **extra_args + ) + if tm or tc: + env.reset() + finally: + env.close() diff --git a/metadrive/manager/scenario_agent_manager.py b/metadrive/manager/scenario_agent_manager.py new file mode 100644 index 000000000..eed37e279 --- /dev/null +++ b/metadrive/manager/scenario_agent_manager.py @@ -0,0 +1,8 @@ +from metadrive.manager.agent_manager import VehicleAgentManager + + +class ScenarioAgentManager(VehicleAgentManager): + """ + PZH: I should note that the ego car's information is filled by scenario_map_manager. So we don't have to do anything + here. + """ \ No newline at end of file diff --git a/metadrive/manager/scenario_map_manager.py b/metadrive/manager/scenario_map_manager.py index 5a4fd51c0..c22277fe4 100644 --- a/metadrive/manager/scenario_map_manager.py +++ b/metadrive/manager/scenario_map_manager.py @@ -53,9 +53,16 @@ def update_route(self): sdc_traj = parse_full_trajectory(sdc_track) - init_state = parse_object_state(sdc_track, 0, check_last_state=False) + init_state = parse_object_state(sdc_track, 0, check_last_state=False, include_z_position=True) + + # TODO(PZH): I hate this but we have to workaround this for a weird nuscenes bug... + if data["version"].startswith("nuscenesv1.0") or data["metadata"]["dataset"] == "nuscenes": + init_state["width"], init_state["length"] = init_state["length"], init_state["width"] + last_state = parse_object_state(sdc_track, -1, check_last_state=True) init_position = init_state["position"] + init_position[-1] = 0 + init_yaw = init_state["heading"] last_position = last_state["position"] last_yaw = last_state["heading"] @@ -69,7 +76,12 @@ def update_route(self): dict( agent_configs={ DEFAULT_AGENT: dict( - spawn_position_heading=(init_position, init_yaw), spawn_velocity=init_state["velocity"] + # Add a fake Z axis so that the object will not fall from the sky. + spawn_position_heading=(list(init_position), init_yaw), + spawn_velocity=init_state["velocity"], + width=init_state["width"], + length=init_state["length"], + height=init_state["height"], ) } ) diff --git a/metadrive/manager/scenario_traffic_manager.py b/metadrive/manager/scenario_traffic_manager.py index ac25d53ef..6966bccce 100644 --- a/metadrive/manager/scenario_traffic_manager.py +++ b/metadrive/manager/scenario_traffic_manager.py @@ -4,11 +4,11 @@ import numpy as np from metadrive.component.static_object.traffic_object import TrafficCone, TrafficBarrier -from metadrive.component.traffic_participants.cyclist import Cyclist -from metadrive.component.traffic_participants.pedestrian import Pedestrian +from metadrive.component.traffic_participants.cyclist import Cyclist, CyclistBoundingBox +from metadrive.component.traffic_participants.pedestrian import Pedestrian, PedestrainBoundingBox from metadrive.component.vehicle.base_vehicle import BaseVehicle from metadrive.component.vehicle.vehicle_type import SVehicle, LVehicle, MVehicle, XLVehicle, \ - TrafficDefaultVehicle + TrafficDefaultVehicle, VaryingDynamicsBoundingBoxVehicle from metadrive.constants import DEFAULT_AGENT from metadrive.manager.base_manager import BaseManager from metadrive.policy.idm_policy import TrajectoryIDMPolicy @@ -169,7 +169,10 @@ def vehicles(self): return list(self.engine.get_objects(filter=lambda o: isinstance(o, BaseVehicle)).values()) def spawn_vehicle(self, v_id, track): - state = parse_object_state(track, self.episode_step) + state = parse_object_state(track, self.episode_step, include_z_position=False) + + use_bounding_box = self.engine.global_config["vehicle_config"]["vehicle_model" + ] == "varying_dynamics_bounding_box" # for each vehicle, we would like to know if it is static if v_id not in self._static_car_id and v_id not in self._moving_car_id: @@ -184,7 +187,7 @@ def spawn_vehicle(self, v_id, track): # if collision don't generate, unless ego car is in replay mode ego_pos = self.ego_vehicle.position - heading_dist, side_dist = self.ego_vehicle.convert_to_local_coordinates(state["position"], ego_pos) + heading_dist, side_dist = self.ego_vehicle.convert_to_local_coordinates(state["position"][:2], ego_pos) if not self.is_ego_vehicle_replay and self._filter_overlapping_car and \ abs(heading_dist) < self.GENERATION_FORWARD_CONSTRAINT and \ abs(side_dist) < self.GENERATION_SIDE_CONSTRAINT: @@ -195,10 +198,18 @@ def spawn_vehicle(self, v_id, track): vehicle_class = state["vehicle_class"] else: vehicle_class = get_vehicle_type( - float(state["length"]), None if self.even_sample_v else self.np_random, self.need_default_vehicle + float(state["length"]), + None if self.even_sample_v else self.np_random, + self.need_default_vehicle, + use_bounding_box=use_bounding_box ) obj_name = v_id if self.engine.global_config["force_reuse_object_name"] else None v_cfg = copy.copy(self._traffic_v_config) + if use_bounding_box: + v_cfg["width"] = state["width"] + v_cfg["length"] = state["length"] + v_cfg["height"] = state["height"] + if self.engine.global_config["top_down_show_real_size"]: v_cfg["top_down_length"] = track["state"]["length"][self.episode_step] v_cfg["top_down_width"] = track["state"]["width"][self.episode_step] @@ -207,8 +218,19 @@ def spawn_vehicle(self, v_id, track): "Scenario ID: {}. The top_down size of vehicle {} is weird: " "{}".format(self.engine.current_seed, v_id, [v_cfg["length"], v_cfg["width"]]) ) + + position = list(state["position"]) + + # Add z to make it stick to the ground: + position.append(state['height'] / 2) + v = self.spawn_object( - vehicle_class, position=state["position"], heading=state["heading"], vehicle_config=v_cfg, name=obj_name + vehicle_class, + # PZH Note: We are using 3D position (including Z) to spawn object. + position=position, + heading=state["heading"], + vehicle_config=v_cfg, + name=obj_name ) self._scenario_id_to_obj_id[v_id] = v.name self._obj_id_to_scenario_id[v.name] = v_id @@ -236,15 +258,27 @@ def spawn_vehicle(self, v_id, track): self.idm_policy_count += 1 def spawn_pedestrian(self, scenario_id, track): - state = parse_object_state(track, self.episode_step) + state = parse_object_state(track, self.episode_step, include_z_position=False) if not state["valid"]: return obj_name = scenario_id if self.engine.global_config["force_reuse_object_name"] else None + if self.global_config["use_bounding_box"]: + cls = PedestrainBoundingBox + force_spawn = True + else: + cls = Pedestrian + force_spawn = False + + position = list(state["position"]) obj = self.spawn_object( - Pedestrian, + cls, name=obj_name, - position=state["position"], + position=position, heading_theta=state["heading"], + width=state["width"], + length=state["length"], + height=state["height"], + force_spawn=force_spawn ) self._scenario_id_to_obj_id[scenario_id] = obj.name self._obj_id_to_scenario_id[obj.name] = scenario_id @@ -252,15 +286,27 @@ def spawn_pedestrian(self, scenario_id, track): policy.act() def spawn_cyclist(self, scenario_id, track): - state = parse_object_state(track, self.episode_step) + state = parse_object_state(track, self.episode_step, include_z_position=False) if not state["valid"]: return obj_name = scenario_id if self.engine.global_config["force_reuse_object_name"] else None + if self.global_config["use_bounding_box"]: + cls = CyclistBoundingBox + force_spawn = True + else: + cls = Cyclist + force_spawn = False + + position = list(state["position"]) obj = self.spawn_object( - Cyclist, + cls, name=obj_name, - position=state["position"], + position=position, heading_theta=state["heading"], + width=state["width"], + length=state["length"], + height=state["height"], + force_spawn=force_spawn ) self._scenario_id_to_obj_id[scenario_id] = obj.name self._obj_id_to_scenario_id[obj.name] = scenario_id @@ -336,7 +382,9 @@ def get_traffic_v_config(): type_count = [0 for i in range(3)] -def get_vehicle_type(length, np_random=None, need_default_vehicle=False): +def get_vehicle_type(length, np_random=None, need_default_vehicle=False, use_bounding_box=False): + if use_bounding_box: + return VaryingDynamicsBoundingBoxVehicle if np_random is not None: if length <= 4: return SVehicle diff --git a/metadrive/policy/replay_policy.py b/metadrive/policy/replay_policy.py index 80f54d8a7..28ccce036 100644 --- a/metadrive/policy/replay_policy.py +++ b/metadrive/policy/replay_policy.py @@ -63,6 +63,7 @@ def act(self, *args, **kwargs): self.control_object.set_velocity(info["velocity"], in_local_frame=self._velocity_local_frame) self.control_object.set_heading_theta(info["heading"]) self.control_object.set_angular_velocity(info["angular_velocity"]) + self.control_object.set_static(True) return None # Return None action so the base vehicle will not overwrite the steering & throttle diff --git a/metadrive/scenario/parse_object_state.py b/metadrive/scenario/parse_object_state.py index 6675ae03e..8078d2fb7 100644 --- a/metadrive/scenario/parse_object_state.py +++ b/metadrive/scenario/parse_object_state.py @@ -25,7 +25,7 @@ def get_idm_route(traj_points, width=2): return traj -def parse_object_state(object_dict, time_idx, check_last_state=False, sim_time_interval=0.1): +def parse_object_state(object_dict, time_idx, check_last_state=False, sim_time_interval=0.1, include_z_position=False): """ Parse object state of one time step """ @@ -47,7 +47,11 @@ def parse_object_state(object_dict, time_idx, check_last_state=False, sim_time_i ret = {k: v[time_idx] for k, v in states.items()} - ret["position"] = states["position"][time_idx, :2] + if include_z_position: + ret["position"] = states["position"][time_idx] + else: + ret["position"] = states["position"][time_idx, :2] + ret["velocity"] = states["velocity"][time_idx] ret["heading_theta"] = states["heading"][time_idx] diff --git a/try.py b/try.py new file mode 100755 index 000000000..78d9db82a --- /dev/null +++ b/try.py @@ -0,0 +1,83 @@ +# visualization +# from IPython.display import Image as IImage +import pygame +import numpy as np +from PIL import Image + +def make_GIF(frames, name="demo.gif"): + print("Generate gif...") + imgs = [frame for frame in frames] + imgs = [Image.fromarray(img) for img in imgs] + imgs[0].save(name, save_all=True, append_images=imgs[1:], duration=50, loop=0) + + #@title Make some configurations and import some modules +from metadrive.engine.engine_utils import close_engine +close_engine() +from metadrive.pull_asset import pull_asset +pull_asset(False) +# NOTE: usually you don't need the above lines. It is only for avoiding a potential bug when running on colab + +from metadrive.engine.asset_loader import AssetLoader +from metadrive.policy.replay_policy import ReplayEgoCarPolicy +from metadrive.policy.idm_policy import IDMPolicy +from metadrive.envs.scenario_env import ScenarioEnv +from metadrive.envs.metadrive_env import MetaDriveEnv +import os + +threeD_render=False # turn on this to enable 3D render. It only works when you have a screen and not running on Colab. +threeD_render=threeD_render and not RunningInCOLAB +os.environ["SDL_VIDEODRIVER"] = "dummy" # Hide the pygame window +waymo_data = AssetLoader.file_path(AssetLoader.asset_path, "waymo", unix_style=False) # Use the built-in datasets with simulator +nuscenes_data = AssetLoader.file_path(AssetLoader.asset_path, "nuscenes", unix_style=False) # Use the built-in datasets with simulator + +import pygame +from metadrive.component.sensors.semantic_camera import SemanticCamera +from metadrive.component.sensors.depth_camera import DepthCamera +from metadrive.component.sensors.rgb_camera import RGBCamera +from metadrive.engine.asset_loader import AssetLoader + + +sensor_size = (84, 60) if os.getenv('TEST_DOC') else (200, 100) +from metadrive.component.sensors.rgb_camera import RGBCamera +# env = ScenarioEnv( +# { +# # "manual_control": False, +# # "reactive_traffic": False, +# # "use_render": threeD_render, +# "agent_policy": ReplayEgoCarPolicy, +# "data_directory": waymo_data, +# "num_scenarios": 1, +# "image_observation":True, +# "vehicle_config":dict(image_source="rgb_camera"), +# "sensors":{"rgb_camera": (RGBCamera, *sensor_size)}, +# "stack_size":3, +# } +# ) + +cfg=dict(image_observation=True, + vehicle_config=dict(image_source="rgb_camera"), + sensors={"rgb_camera": (RGBCamera, *sensor_size)}, + stack_size=3, + agent_policy=IDMPolicy # drive with IDM policy + ) + +env=MetaDriveEnv(cfg) + +# @title Run Simulation + +env.reset(seed=0) +frames = [] +for i in range(1, 100000): + o, r, tm, tc, info = env.step([1.0, 0.]) + frames.append(env.render(mode="topdown",film_size=(1200, 1200))) + # ret=o["image"][..., -1]*255 # [0., 1.] to [0, 255] + # ret=ret.astype(np.uint8) + # frames.append(ret[..., ::-1]) + print(o) + if tm or tc: + break +env.close() + +make_GIF(frames) +# visualization +Image(open("demo.gif", 'rb').read()) \ No newline at end of file