From 12236bfb0a4c968c04575fa009b4163784b8c718 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Tue, 26 Mar 2024 18:03:59 -0700 Subject: [PATCH 01/17] Introducing the bounding box models --- metadrive/component/vehicle/vehicle_type.py | 94 ++++++++++++++++--- metadrive/envs/scenario_env.py | 9 +- .../drive_in_real_env_with_bounding_box.py | 59 ++++++++++++ metadrive/manager/scenario_agent_manager.py | 8 ++ metadrive/manager/scenario_map_manager.py | 11 ++- metadrive/manager/scenario_traffic_manager.py | 19 +++- 6 files changed, 182 insertions(+), 18 deletions(-) create mode 100755 metadrive/examples/drive_in_real_env_with_bounding_box.py create mode 100644 metadrive/manager/scenario_agent_manager.py diff --git a/metadrive/component/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index 98eab0a08..258aa6f04 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -1,8 +1,11 @@ import platform -from metadrive.component.pg_space import ParameterSpace, VehicleParameterSpace +from panda3d.core import Material, Vec3 + +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 class DefaultVehicle(BaseVehicle): @@ -208,25 +211,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 +269,74 @@ def reset( return ret +class VaryingDynamicsBoundingBoxVehicle(VaryingDynamicsVehicle): + 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) + # model default, face to y + car_model.setHpr(*HPR) + + # else: + # car_model = BaseVehicle.model_collection[path] + + car_model.instanceTo(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 +360,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/envs/scenario_env.py b/metadrive/envs/scenario_env.py index 7831742be..76295dbfe 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 @@ -115,6 +115,9 @@ def __init__(self, config=None): self.start_index = self.config["start_scenario_index"] self.num_scenarios = self.config["num_scenarios"] + 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 +182,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..e0d0be2c5 --- /dev/null +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -0,0 +1,59 @@ +#!/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, + "vehicle_config": dict(random_color=True, vehicle_model="varying_dynamics_bounding_box"), + "agent_configs": { + "default_agent": dict(use_special_color=True, vehicle_model="varying_dynamics_bounding_box") + }, + } + ) + 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..c0f465ab6 100644 --- a/metadrive/manager/scenario_map_manager.py +++ b/metadrive/manager/scenario_map_manager.py @@ -54,6 +54,11 @@ def update_route(self): sdc_traj = parse_full_trajectory(sdc_track) init_state = parse_object_state(sdc_track, 0, check_last_state=False) + + # 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_yaw = init_state["heading"] @@ -69,7 +74,11 @@ def update_route(self): dict( agent_configs={ DEFAULT_AGENT: dict( - spawn_position_heading=(init_position, init_yaw), spawn_velocity=init_state["velocity"] + spawn_position_heading=(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..a23b7aea2 100644 --- a/metadrive/manager/scenario_traffic_manager.py +++ b/metadrive/manager/scenario_traffic_manager.py @@ -8,7 +8,7 @@ from metadrive.component.traffic_participants.pedestrian import Pedestrian 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 @@ -171,6 +171,9 @@ def vehicles(self): def spawn_vehicle(self, v_id, track): state = parse_object_state(track, self.episode_step) + 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: valid_points = track["state"]["position"][np.where(track["state"]["valid"])] @@ -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] @@ -336,7 +347,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 From 30a1a0273d0019308291624962f049283e27b55d Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 27 Mar 2024 11:06:31 -0700 Subject: [PATCH 02/17] Now cyclist and pedestrain are both bounding box if config["use_bounding_box"] is True for ScenarioEnv --- .../base_traffic_participant.py | 4 +- .../component/traffic_participants/cyclist.py | 119 +++++++++++++++++- .../traffic_participants/pedestrian.py | 115 ++++++++++++++++- metadrive/envs/scenario_env.py | 12 +- .../drive_in_real_env_with_bounding_box.py | 5 +- metadrive/manager/scenario_traffic_manager.py | 28 ++++- 6 files changed, 267 insertions(+), 16 deletions(-) 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..d7682dd1a 100644 --- a/metadrive/component/traffic_participants/cyclist.py +++ b/metadrive/component/traffic_participants/cyclist.py @@ -9,7 +9,6 @@ from metadrive.engine.asset_loader import AssetLoader from metadrive.engine.physics_node import BaseRigidBodyNode - class Cyclist(BaseTrafficParticipant): MASS = 80 # kg TYPE_NAME = MetaDriveType.CYCLIST @@ -19,13 +18,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 +44,117 @@ 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"] + } + 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 + 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"] + + @property + def HEIGHT(self): + return self.config["height"] + + @property + def LENGTH(self): + return self.config["length"] diff --git a/metadrive/component/traffic_participants/pedestrian.py b/metadrive/component/traffic_participants/pedestrian.py index fed51f295..3122e99fe 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 @@ -21,7 +21,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 +116,114 @@ 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, 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 = (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/envs/scenario_env.py b/metadrive/envs/scenario_env.py index 76295dbfe..7811c6c4f 100644 --- a/metadrive/envs/scenario_env.py +++ b/metadrive/envs/scenario_env.py @@ -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,15 @@ 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()) diff --git a/metadrive/examples/drive_in_real_env_with_bounding_box.py b/metadrive/examples/drive_in_real_env_with_bounding_box.py index e0d0be2c5..c0f4c86df 100755 --- a/metadrive/examples/drive_in_real_env_with_bounding_box.py +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -38,10 +38,7 @@ ), "num_scenarios": 3 if use_waymo else 10, "debug": True, - "vehicle_config": dict(random_color=True, vehicle_model="varying_dynamics_bounding_box"), - "agent_configs": { - "default_agent": dict(use_special_color=True, vehicle_model="varying_dynamics_bounding_box") - }, + "use_bounding_box": True, } ) o, _ = env.reset() diff --git a/metadrive/manager/scenario_traffic_manager.py b/metadrive/manager/scenario_traffic_manager.py index a23b7aea2..7df72c39a 100644 --- a/metadrive/manager/scenario_traffic_manager.py +++ b/metadrive/manager/scenario_traffic_manager.py @@ -4,8 +4,8 @@ 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, VaryingDynamicsBoundingBoxVehicle @@ -251,11 +251,21 @@ def spawn_pedestrian(self, scenario_id, track): 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 obj = self.spawn_object( - Pedestrian, + cls, name=obj_name, position=state["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 @@ -267,11 +277,21 @@ def spawn_cyclist(self, scenario_id, track): 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 obj = self.spawn_object( - Cyclist, + cls, name=obj_name, position=state["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 From 152f176d63c513a38c1f6de9b3281bd514767c20 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 27 Mar 2024 11:06:52 -0700 Subject: [PATCH 03/17] format --- .../component/traffic_participants/cyclist.py | 19 +++++-------------- .../traffic_participants/pedestrian.py | 9 ++------- metadrive/envs/scenario_env.py | 4 ++-- 3 files changed, 9 insertions(+), 23 deletions(-) diff --git a/metadrive/component/traffic_participants/cyclist.py b/metadrive/component/traffic_participants/cyclist.py index d7682dd1a..39efbd36d 100644 --- a/metadrive/component/traffic_participants/cyclist.py +++ b/metadrive/component/traffic_participants/cyclist.py @@ -9,6 +9,7 @@ from metadrive.engine.asset_loader import AssetLoader from metadrive.engine.physics_node import BaseRigidBodyNode + class Cyclist(BaseTrafficParticipant): MASS = 80 # kg TYPE_NAME = MetaDriveType.CYCLIST @@ -60,11 +61,7 @@ class CyclistBoundingBox(BaseTrafficParticipant): 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["width"], "length": kwargs["length"], "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) @@ -89,8 +86,7 @@ def __init__(self, position, heading_theta, random_seed, name=None, **kwargs): material = Material() material.setBaseColor( ( - self.panda_color[0] * self.MATERIAL_COLOR_COEFF, - self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + 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. ) ) @@ -104,11 +100,7 @@ def __init__(self, position, heading_theta, random_seed, name=None, **kwargs): 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"] - } + config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]} self.update_config(config) if self._instance is not None: self._instance.detachNode() @@ -130,8 +122,7 @@ def reset(self, position, heading_theta: float = 0., random_seed=None, name=None material = Material() material.setBaseColor( ( - self.panda_color[0] * self.MATERIAL_COLOR_COEFF, - self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + 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. ) ) diff --git a/metadrive/component/traffic_participants/pedestrian.py b/metadrive/component/traffic_participants/pedestrian.py index 3122e99fe..52d6b2b2b 100644 --- a/metadrive/component/traffic_participants/pedestrian.py +++ b/metadrive/component/traffic_participants/pedestrian.py @@ -165,8 +165,7 @@ def __init__(self, position, heading_theta, width, length, height, random_seed=N material = Material() material.setBaseColor( ( - self.panda_color[0] * self.MATERIAL_COLOR_COEFF, - self.panda_color[1] * self.MATERIAL_COLOR_COEFF, + 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. ) ) @@ -180,11 +179,7 @@ def __init__(self, position, heading_theta, width, length, height, random_seed=N 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"] - } + config = {"width": kwargs["width"], "length": kwargs["length"], "height": kwargs["height"]} self.update_config(config) if self._instance is not None: self._instance.detachNode() diff --git a/metadrive/envs/scenario_env.py b/metadrive/envs/scenario_env.py index 7811c6c4f..475d91fca 100644 --- a/metadrive/envs/scenario_env.py +++ b/metadrive/envs/scenario_env.py @@ -119,8 +119,8 @@ def __init__(self, config=None): 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["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 From 3bcb7b56974d7deea922f3c79ffd449b7ed7bab9 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 27 Mar 2024 11:34:39 -0700 Subject: [PATCH 04/17] fix the problem of init height --- metadrive/manager/scenario_map_manager.py | 7 ++++-- metadrive/manager/scenario_traffic_manager.py | 24 +++++++++++++------ metadrive/scenario/parse_object_state.py | 8 +++++-- 3 files changed, 28 insertions(+), 11 deletions(-) diff --git a/metadrive/manager/scenario_map_manager.py b/metadrive/manager/scenario_map_manager.py index c0f465ab6..c22277fe4 100644 --- a/metadrive/manager/scenario_map_manager.py +++ b/metadrive/manager/scenario_map_manager.py @@ -53,7 +53,7 @@ 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": @@ -61,6 +61,8 @@ def update_route(self): 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"] @@ -74,7 +76,8 @@ def update_route(self): dict( agent_configs={ DEFAULT_AGENT: dict( - spawn_position_heading=(init_position, init_yaw), + # 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"], diff --git a/metadrive/manager/scenario_traffic_manager.py b/metadrive/manager/scenario_traffic_manager.py index 7df72c39a..ab3412122 100644 --- a/metadrive/manager/scenario_traffic_manager.py +++ b/metadrive/manager/scenario_traffic_manager.py @@ -169,7 +169,7 @@ 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=True) use_bounding_box = self.engine.global_config["vehicle_config"]["vehicle_model" ] == "varying_dynamics_bounding_box" @@ -187,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: @@ -218,8 +218,14 @@ 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"]) + position[-1] /= 2 # half height 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 @@ -247,7 +253,7 @@ 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 @@ -257,10 +263,12 @@ def spawn_pedestrian(self, scenario_id, track): else: cls = Pedestrian force_spawn = False + + position = list(state["position"]) obj = self.spawn_object( cls, name=obj_name, - position=state["position"], + position=position, heading_theta=state["heading"], width=state["width"], length=state["length"], @@ -273,7 +281,7 @@ 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 @@ -283,10 +291,12 @@ def spawn_cyclist(self, scenario_id, track): else: cls = Cyclist force_spawn = False + + position = list(state["position"]) obj = self.spawn_object( cls, name=obj_name, - position=state["position"], + position=position, heading_theta=state["heading"], width=state["width"], length=state["length"], 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] From 9f1afbcf4cb30900340d04b514e1f1ada12c7ad0 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 27 Mar 2024 11:34:51 -0700 Subject: [PATCH 05/17] format --- metadrive/manager/scenario_traffic_manager.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/metadrive/manager/scenario_traffic_manager.py b/metadrive/manager/scenario_traffic_manager.py index ab3412122..33b8b96a5 100644 --- a/metadrive/manager/scenario_traffic_manager.py +++ b/metadrive/manager/scenario_traffic_manager.py @@ -225,7 +225,9 @@ def spawn_vehicle(self, v_id, track): 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 + 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 From a54f7fdc9a3d9f84774e20be8143a3670920149c Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Thu, 28 Mar 2024 07:58:28 -0700 Subject: [PATCH 06/17] fix semantic for bounding box pedestrian --- metadrive/component/sensors/semantic_camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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, From 68400813a544ec906b577f7c09f36530f5316eb2 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Sun, 31 Mar 2024 07:46:16 -0700 Subject: [PATCH 07/17] up --- metadrive/component/traffic_participants/pedestrian.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/metadrive/component/traffic_participants/pedestrian.py b/metadrive/component/traffic_participants/pedestrian.py index 52d6b2b2b..5b247c677 100644 --- a/metadrive/component/traffic_participants/pedestrian.py +++ b/metadrive/component/traffic_participants/pedestrian.py @@ -154,7 +154,7 @@ def __init__(self, position, heading_theta, width, length, height, random_seed=N self._instance = model.instanceTo(self.origin) # Add some color to help debug - from panda3d.core import Material, LVecBase4 + from panda3d.core import Material import seaborn as sns color = sns.color_palette("colorblind") color.remove(color[2]) # Remove the green and leave it for special vehicle From 8d496aacf5bb6257a4d9d2ad1468b1607ee8bb5f Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Tue, 9 Apr 2024 17:34:50 -0700 Subject: [PATCH 08/17] fix the navi mark line / dest mark line in Trajectory navigation --- .../navigation_module/trajectory_navigation.py | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/metadrive/component/navigation_module/trajectory_navigation.py b/metadrive/component/navigation_module/trajectory_navigation.py index 308c159ae..69d8cc350 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,18 @@ 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 From 55d028577605098d6fcaf28a715907e59c571a33 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Tue, 9 Apr 2024 17:35:01 -0700 Subject: [PATCH 09/17] Draw line at the bottom of the bounding box --- metadrive/component/vehicle/vehicle_type.py | 58 +++++++++++++++++-- .../drive_in_real_env_with_bounding_box.py | 5 ++ 2 files changed, 58 insertions(+), 5 deletions(-) diff --git a/metadrive/component/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index 258aa6f04..617247cc5 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -1,12 +1,12 @@ import platform -from panda3d.core import Material, Vec3 +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): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.DEFAULT_VEHICLE) @@ -270,6 +270,35 @@ def reset( 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 @@ -279,17 +308,36 @@ def _add_visualization(self): # 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) # model default, face to y car_model.setHpr(*HPR) + car_model.instanceTo(self.origin) - # else: - # car_model = BaseVehicle.model_collection[path] + # ========== 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(5) + 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_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) - car_model.instanceTo(self.origin) if self.config["random_color"]: material = Material() material.setBaseColor( diff --git a/metadrive/examples/drive_in_real_env_with_bounding_box.py b/metadrive/examples/drive_in_real_env_with_bounding_box.py index c0f4c86df..351e3f380 100755 --- a/metadrive/examples/drive_in_real_env_with_bounding_box.py +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -39,6 +39,11 @@ "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, + } } ) o, _ = env.reset() From 26047010783317906adcb90df603051dd097b389 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Tue, 9 Apr 2024 17:35:16 -0700 Subject: [PATCH 10/17] format --- metadrive/component/navigation_module/trajectory_navigation.py | 1 - metadrive/component/vehicle/vehicle_type.py | 2 +- metadrive/examples/drive_in_real_env_with_bounding_box.py | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/metadrive/component/navigation_module/trajectory_navigation.py b/metadrive/component/navigation_module/trajectory_navigation.py index 69d8cc350..e36482391 100644 --- a/metadrive/component/navigation_module/trajectory_navigation.py +++ b/metadrive/component/navigation_module/trajectory_navigation.py @@ -156,7 +156,6 @@ def update_localization(self, ego_vehicle): 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/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index 617247cc5..d1b3c7bab 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -8,6 +8,7 @@ from metadrive.engine.asset_loader import AssetLoader from panda3d.core import TransparencyAttrib, LineSegs, NodePath, BoundingHexahedron + class DefaultVehicle(BaseVehicle): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.DEFAULT_VEHICLE) # LENGTH = 4.51 @@ -298,7 +299,6 @@ class VaryingDynamicsBoundingBoxVehicle(VaryingDynamicsVehicle): # # return r - def _add_visualization(self): if self.render: [path, scale, offset, HPR] = self.path diff --git a/metadrive/examples/drive_in_real_env_with_bounding_box.py b/metadrive/examples/drive_in_real_env_with_bounding_box.py index 351e3f380..4ac309001 100755 --- a/metadrive/examples/drive_in_real_env_with_bounding_box.py +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -39,7 +39,6 @@ "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, From 2d39dd0021a31063fb2dcdc6269dfaaf52ec1f48 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 10 Apr 2024 13:48:56 -0700 Subject: [PATCH 11/17] introducing the disable_collision config --- metadrive/constants.py | 4 +++- metadrive/engine/core/engine_core.py | 4 +++- metadrive/engine/core/physics_world.py | 6 +++--- metadrive/envs/base_env.py | 2 ++ metadrive/examples/drive_in_real_env_with_bounding_box.py | 4 +++- 5 files changed, 14 insertions(+), 6 deletions(-) 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/examples/drive_in_real_env_with_bounding_box.py b/metadrive/examples/drive_in_real_env_with_bounding_box.py index 4ac309001..82d757e3e 100755 --- a/metadrive/examples/drive_in_real_env_with_bounding_box.py +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -42,7 +42,9 @@ "vehicle_config": { "show_line_to_dest": True, "show_line_to_navi_mark": True, - } + }, + + "disable_collision": True, } ) o, _ = env.reset() From a6cce69863795fcdeb0b1820fb9645ecb60450f7 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 10 Apr 2024 13:49:13 -0700 Subject: [PATCH 12/17] set replay object to static (so it will never fall) --- metadrive/policy/replay_policy.py | 1 + 1 file changed, 1 insertion(+) 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 From 627d7b4e6e6fcc14628b2f1f696112d6de02c393 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 10 Apr 2024 13:49:52 -0700 Subject: [PATCH 13/17] format --- metadrive/examples/drive_in_real_env_with_bounding_box.py | 1 - 1 file changed, 1 deletion(-) diff --git a/metadrive/examples/drive_in_real_env_with_bounding_box.py b/metadrive/examples/drive_in_real_env_with_bounding_box.py index 82d757e3e..b26142a0a 100755 --- a/metadrive/examples/drive_in_real_env_with_bounding_box.py +++ b/metadrive/examples/drive_in_real_env_with_bounding_box.py @@ -43,7 +43,6 @@ "show_line_to_dest": True, "show_line_to_navi_mark": True, }, - "disable_collision": True, } ) From 04932510c91d3461fd14f0147fd224aede86df80 Mon Sep 17 00:00:00 2001 From: pengzhenghao Date: Wed, 10 Apr 2024 15:59:38 -0700 Subject: [PATCH 14/17] let the vehicle stick to the ground --- metadrive/component/vehicle/vehicle_type.py | 3 ++- metadrive/manager/scenario_traffic_manager.py | 7 +++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/metadrive/component/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index d1b3c7bab..581b42698 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -312,7 +312,8 @@ def _add_visualization(self): 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(-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) diff --git a/metadrive/manager/scenario_traffic_manager.py b/metadrive/manager/scenario_traffic_manager.py index 33b8b96a5..6966bccce 100644 --- a/metadrive/manager/scenario_traffic_manager.py +++ b/metadrive/manager/scenario_traffic_manager.py @@ -169,7 +169,7 @@ 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, include_z_position=True) + 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" @@ -220,7 +220,10 @@ def spawn_vehicle(self, v_id, track): ) position = list(state["position"]) - position[-1] /= 2 # half height + + # Add z to make it stick to the ground: + position.append(state['height'] / 2) + v = self.spawn_object( vehicle_class, # PZH Note: We are using 3D position (including Z) to spawn object. From cf8d287dc76faf9ffdfade085a192157f60daf59 Mon Sep 17 00:00:00 2001 From: Yunsong Zhou Date: Thu, 11 Apr 2024 15:17:09 -0700 Subject: [PATCH 15/17] add vehicle box and inverse cyclist length/width --- .../component/traffic_participants/cyclist.py | 6 +- metadrive/component/vehicle/vehicle_type.py | 26 +++++- try.py | 83 +++++++++++++++++++ 3 files changed, 111 insertions(+), 4 deletions(-) create mode 100755 try.py diff --git a/metadrive/component/traffic_participants/cyclist.py b/metadrive/component/traffic_participants/cyclist.py index 39efbd36d..29e992b8f 100644 --- a/metadrive/component/traffic_participants/cyclist.py +++ b/metadrive/component/traffic_participants/cyclist.py @@ -140,7 +140,8 @@ def set_velocity(self, direction, value=None, in_local_frame=False): @property def WIDTH(self): - return self.config["width"] + # return self.config["width"] + return self.config["length"] @property def HEIGHT(self): @@ -148,4 +149,5 @@ def HEIGHT(self): @property def LENGTH(self): - return self.config["length"] + # return self.config["length"] + return self.config["width"] diff --git a/metadrive/component/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index 581b42698..601e5b6b5 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -322,17 +322,39 @@ def _add_visualization(self): # Draw the bottom of the car first line_seg = LineSegs("bounding_box_contour1") zoffset = car_model.getZ() - line_seg.setThickness(5) + 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.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)) 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 From f2a932642ba7238cc8ef921f563c1fe30414a262 Mon Sep 17 00:00:00 2001 From: Yunsong Zhou Date: Thu, 11 Apr 2024 16:11:14 -0700 Subject: [PATCH 16/17] draw cross line and fix cyclist length/width --- .../component/traffic_participants/cyclist.py | 55 ++++++++++++++++++- .../traffic_participants/pedestrian.py | 52 +++++++++++++++++- 2 files changed, 105 insertions(+), 2 deletions(-) diff --git a/metadrive/component/traffic_participants/cyclist.py b/metadrive/component/traffic_participants/cyclist.py index 29e992b8f..ecb3b7556 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 @@ -62,6 +63,7 @@ class CyclistBoundingBox(BaseTrafficParticipant): 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) @@ -71,12 +73,63 @@ def __init__(self, position, heading_theta, random_seed, name=None, **kwargs): if self.render: model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) + # model.setScale((self.LENGTH, self.WIDTH, self.LENGTH)) + print("CyclistBoundingBox: ", self.LENGTH, self.WIDTH, 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 + + # ========== 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 diff --git a/metadrive/component/traffic_participants/pedestrian.py b/metadrive/component/traffic_participants/pedestrian.py index 5b247c677..1809112d9 100644 --- a/metadrive/component/traffic_participants/pedestrian.py +++ b/metadrive/component/traffic_participants/pedestrian.py @@ -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 @@ -156,6 +157,55 @@ def __init__(self, position, heading_theta, width, length, height, random_seed=N # Add some color to help debug from panda3d.core import Material import seaborn as sns + + # ========== 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 From 07a31a28809e4cb461711223563d80eabc712873 Mon Sep 17 00:00:00 2001 From: Yunsong Zhou Date: Thu, 11 Apr 2024 16:27:30 -0700 Subject: [PATCH 17/17] draw cross line and fix cyclist length/width --- .../component/traffic_participants/cyclist.py | 98 +++++++++---------- .../traffic_participants/pedestrian.py | 96 +++++++++--------- metadrive/component/vehicle/vehicle_type.py | 86 ++++++++-------- 3 files changed, 142 insertions(+), 138 deletions(-) diff --git a/metadrive/component/traffic_participants/cyclist.py b/metadrive/component/traffic_participants/cyclist.py index ecb3b7556..f5440d6cd 100644 --- a/metadrive/component/traffic_participants/cyclist.py +++ b/metadrive/component/traffic_participants/cyclist.py @@ -73,8 +73,6 @@ def __init__(self, position, heading_theta, random_seed, name=None, **kwargs): if self.render: model = AssetLoader.loader.loadModel(AssetLoader.file_path("models", "box.bam")) model.setScale((self.WIDTH, self.LENGTH, self.HEIGHT)) - # model.setScale((self.LENGTH, self.WIDTH, self.LENGTH)) - print("CyclistBoundingBox: ", self.LENGTH, self.WIDTH, self.HEIGHT) model.setTwoSided(False) self._instance = model.instanceTo(self.origin) @@ -82,53 +80,55 @@ def __init__(self, position, heading_theta, random_seed, name=None, **kwargs): from panda3d.core import Material, LVecBase4 import seaborn as sns - # ========== 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) + 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 diff --git a/metadrive/component/traffic_participants/pedestrian.py b/metadrive/component/traffic_participants/pedestrian.py index 1809112d9..5955f57e2 100644 --- a/metadrive/component/traffic_participants/pedestrian.py +++ b/metadrive/component/traffic_participants/pedestrian.py @@ -158,53 +158,55 @@ def __init__(self, position, heading_theta, width, length, height, random_seed=N from panda3d.core import Material import seaborn as sns - # ========== 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) + 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 diff --git a/metadrive/component/vehicle/vehicle_type.py b/metadrive/component/vehicle/vehicle_type.py index 601e5b6b5..d4de1189c 100644 --- a/metadrive/component/vehicle/vehicle_type.py +++ b/metadrive/component/vehicle/vehicle_type.py @@ -318,48 +318,50 @@ def _add_visualization(self): car_model.setHpr(*HPR) car_model.instanceTo(self.origin) - # ========== 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) + 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()