From 9010dd28c18e181dfbdce095e6476720e8f2b3b8 Mon Sep 17 00:00:00 2001 From: Tucker Alban Date: Mon, 4 Dec 2023 14:39:55 -0500 Subject: [PATCH] Bugfix vehicle configuration (#2117) * Fix vehicle configuration issues. * Fix bugs. * Update changelog. * Make format. * Add docstring. --- CHANGELOG.md | 4 ++ smarts/core/actor_capture_manager.py | 2 - smarts/core/agent_interface.py | 16 +++++- smarts/core/agent_manager.py | 2 - smarts/core/vehicle.py | 83 +++++++++++++++++++++------- smarts/core/vehicle_index.py | 35 +++++++++--- smarts/engine.ini | 1 + 7 files changed, 106 insertions(+), 37 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index be14f16214..9a5e98481f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,8 +11,12 @@ Copy and pasting the git commit messages is __NOT__ enough. ## [Unreleased] - XXXX-XX-XX ### Added ### Changed +- `VehicleIndex.build_agent_vehicle()` no longer has `filename` and `surface_patches` parameters. ### Deprecated ### Fixed +- Fixed an issue where `AgentInterface.vehicle_type` would not affect agent vehicles when attempting to take over an existing vehicle. +- Fixed a case where newly created agent vehicles would have a constant `"sedan"` size instead of the size of `AgentInterface.vehicle_type`. +- Fixed a case where if vehicles are replaced they would not respect controller and vehicle parameters. - Fixed an issue where `RandomRoute` would always give the same route across traffic groups in scenario studio. - Fixed an issue where `SMARTS` might not be explicitly destroyed in the `ros_driver`. - Fixed issue where `SumoTrafficSimulation` could get locked up on reset if a scenario had only 1 map but multiple scenario variations. diff --git a/smarts/core/actor_capture_manager.py b/smarts/core/actor_capture_manager.py index 479ec0e678..6489bf9ee5 100644 --- a/smarts/core/actor_capture_manager.py +++ b/smarts/core/actor_capture_manager.py @@ -68,10 +68,8 @@ def _make_new_vehicle( agent_id, agent_interface, plan, - sim.scenario.vehicle_filepath, sim.scenario.tire_parameters_filepath, True, - sim.scenario.surface_patches, initial_speed=initial_speed, boid=False, ) diff --git a/smarts/core/agent_interface.py b/smarts/core/agent_interface.py index f0818c52b6..ee7b640756 100644 --- a/smarts/core/agent_interface.py +++ b/smarts/core/agent_interface.py @@ -22,7 +22,7 @@ from dataclasses import dataclass, field, replace from enum import IntEnum from functools import cached_property -from typing import List, Optional, Tuple, Union +from typing import List, Literal, Optional, Tuple, Union from smarts.core.controllers.action_space_type import ActionSpaceType from smarts.core.lidar_sensor_params import BasicLidar @@ -320,7 +320,9 @@ class AgentInterface: The choice of action space; this also decides the controller that will be enabled and the chassis type that will be used. """ - vehicle_type: str = "sedan" + vehicle_type: Literal[ + "bus", "coach", "motorcycle", "pedestrian", "sedan", "trailer", "truck" + ] = "sedan" """ The choice of vehicle type. """ @@ -367,7 +369,15 @@ def __post_init__(self): self.lane_positions, LanePositions ) self.signals = AgentInterface._resolve_config(self.signals, Signals) - assert self.vehicle_type in {"sedan", "bus"} + assert self.vehicle_type in { + "bus", + "coach", + "motorcycle", + "pedestrian", + "sedan", + "trailer", + "truck", + } @staticmethod def from_type(requested_type: AgentType, **kwargs): diff --git a/smarts/core/agent_manager.py b/smarts/core/agent_manager.py index 1c29a51fae..1a44fd4acf 100644 --- a/smarts/core/agent_manager.py +++ b/smarts/core/agent_manager.py @@ -611,10 +611,8 @@ def _add_agent( agent_id, agent_interface, plan, - scenario.vehicle_filepath, scenario.tire_parameters_filepath, trainable, - scenario.surface_patches, agent_model.initial_speed, boid=boid, vehicle_id=agent_model.actor_name, diff --git a/smarts/core/vehicle.py b/smarts/core/vehicle.py index c05ee94af6..34449ce7c3 100644 --- a/smarts/core/vehicle.py +++ b/smarts/core/vehicle.py @@ -29,7 +29,7 @@ from smarts.core.agent_interface import AgentInterface from smarts.core.plan import Mission, Plan -from . import models +from . import config, models from .actor import ActorRole from .chassis import AckermannChassis, BoxChassis, Chassis from .colors import SceneColors @@ -72,6 +72,8 @@ def __init__( self._id = id self._chassis: Chassis = chassis + if vehicle_config_type == "sedan": + vehicle_config_type = "passenger" self._vehicle_config_type = vehicle_config_type self._action_space = action_space @@ -246,24 +248,69 @@ def valid(self) -> bool: return self._initialized @staticmethod - def agent_vehicle_dims(mission: Mission) -> Dimensions: + @lru_cache(maxsize=None) + def vehicle_urdf_path(vehicle_type: str, override_path: Optional[str]) -> str: + """Resolve the physics model filepath. + + Args: + vehicle_type (str): The type of the vehicle. + override_path (Optional[str]): The override. + + Raises: + ValueError: The vehicle type is valid. + + Returns: + str: The path to the model `.urdf`. + """ + if (override_path is not None) and os.path.exists(override_path): + return override_path + + if vehicle_type == "sedan": + vehicle_type = "passenger" + + if vehicle_type == "passenger": + urdf_name = "vehicle" + elif vehicle_type in { + "bus", + "coach", + "motorcycle", + "pedestrian", + "trailer", + "truck", + }: + urdf_name = vehicle_type + else: + raise ValueError(f"Vehicle type `{vehicle_type}` does not exist!!!") + + with pkg_resources.path(models, urdf_name + ".urdf") as path: + vehicle_filepath = str(path.absolute()) + + return vehicle_filepath + + @staticmethod + def agent_vehicle_dims( + mission: Mission, default: Optional[str] = None + ) -> Dimensions: """Get the vehicle dimensions from the mission requirements. Args: A mission for the agent. Returns: The mission vehicle spec dimensions XOR the default "passenger" vehicle dimensions. """ + if default == "sedan": + default = "passenger" + default_type = default or config().get_setting( + "resources", "default_agent_vehicle", default="passenger" + ) if mission.vehicle_spec: # mission.vehicle_spec.veh_config_type will always be "passenger" for now, # but we use that value here in case we ever expand our history functionality. vehicle_config_type = mission.vehicle_spec.veh_config_type return Dimensions.copy_with_defaults( mission.vehicle_spec.dimensions, - VEHICLE_CONFIGS[vehicle_config_type].dimensions, + VEHICLE_CONFIGS[vehicle_config_type or default_type].dimensions, ) - # non-history agents can currently only control passenger vehicles. - vehicle_config_type = "passenger" - return VEHICLE_CONFIGS[vehicle_config_type].dimensions + return VEHICLE_CONFIGS[default_type].dimensions @classmethod def build_agent_vehicle( @@ -272,7 +319,7 @@ def build_agent_vehicle( vehicle_id: str, agent_interface: AgentInterface, plan: Plan, - vehicle_filepath: str, + vehicle_filepath: Optional[str], tire_filepath: str, trainable: bool, surface_patches: List[Dict[str, Any]], @@ -281,9 +328,14 @@ def build_agent_vehicle( """Create a new vehicle and set up sensors and planning information as required by the ego agent. """ - mission = plan.mission + urdf_file = cls.vehicle_urdf_path( + vehicle_type=agent_interface.vehicle_type, override_path=vehicle_filepath + ) - chassis_dims = cls.agent_vehicle_dims(mission) + mission = plan.mission + chassis_dims = cls.agent_vehicle_dims( + mission, default=agent_interface.vehicle_type + ) start = mission.start if start.from_front_bumper: @@ -296,18 +348,6 @@ def build_agent_vehicle( start_pose = Pose.from_center(start.position, start.heading) vehicle_color = SceneColors.Agent if trainable else SceneColors.SocialAgent - - if agent_interface.vehicle_type == "sedan": - urdf_name = "vehicle" - elif agent_interface.vehicle_type == "bus": - urdf_name = "bus" - else: - raise Exception("Vehicle type does not exist!!!") - - if (vehicle_filepath is None) or not os.path.exists(vehicle_filepath): - with pkg_resources.path(models, urdf_name + ".urdf") as path: - vehicle_filepath = str(path.absolute()) - controller_parameters = sim.vehicle_index.controller_params_for_vehicle_type( agent_interface.vehicle_type ) @@ -340,6 +380,7 @@ def build_agent_vehicle( id=vehicle_id, chassis=chassis, color=vehicle_color, + vehicle_config_type=agent_interface.vehicle_type, ) return vehicle diff --git a/smarts/core/vehicle_index.py b/smarts/core/vehicle_index.py index eb454ca739..7e9324194a 100644 --- a/smarts/core/vehicle_index.py +++ b/smarts/core/vehicle_index.py @@ -459,7 +459,20 @@ def switch_control_to_agent( vehicle = self._vehicles[vehicle_id] chassis = None if agent_interface and agent_interface.action in sim.dynamic_action_spaces: - chassis = AckermannChassis(pose=vehicle.pose, bullet_client=sim.bc) + chassis = AckermannChassis( + pose=vehicle.pose, + bullet_client=sim.bc, + vehicle_filepath=Vehicle.vehicle_urdf_path( + vehicle_type=agent_interface.vehicle_type, + override_path=sim.scenario.vehicle_filepath, + ), + tire_parameters_filepath=sim.scenario.tire_parameters_filepath, + friction_map=sim.scenario.surface_patches, + controller_parameters=self.controller_params_for_vehicle_type( + agent_interface.vehicle_type + ), + initial_speed=vehicle.speed, + ) else: chassis = BoxChassis( pose=vehicle.pose, @@ -612,7 +625,10 @@ def _switch_control_to_agent_recreate( vehicle.id, agent_interface, plan, - sim.scenario.vehicle_filepath, + Vehicle.vehicle_urdf_path( + vehicle_type=agent_interface.vehicle_type, + override_path=sim.scenario.vehicle_filepath, + ), sim.scenario.tire_parameters_filepath, not hijacking, sim.scenario.surface_patches, @@ -660,12 +676,10 @@ def build_agent_vehicle( agent_id, agent_interface, plan, - filepath, tire_filepath, - trainable, - surface_patches, - initial_speed=None, - boid=False, + trainable: bool, + initial_speed: Optional[float] = None, + boid: bool = False, *, vehicle_id=None, ): @@ -675,10 +689,13 @@ def build_agent_vehicle( vehicle_id=vehicle_id or agent_id, agent_interface=agent_interface, plan=plan, - vehicle_filepath=filepath, + vehicle_filepath=Vehicle.vehicle_urdf_path( + vehicle_type=agent_interface.vehicle_type, + override_path=sim.scenario.vehicle_filepath, + ), tire_filepath=tire_filepath, trainable=trainable, - surface_patches=surface_patches, + surface_patches=sim.scenario.surface_patches, initial_speed=initial_speed, ) diff --git a/smarts/engine.ini b/smarts/engine.ini index d28678a871..9f3100c7e8 100644 --- a/smarts/engine.ini +++ b/smarts/engine.ini @@ -9,5 +9,6 @@ max_pybullet_freq = 240 [providers] [rendering] [resources] +default_agent_vehicle = passenger [ray] log_to_driver=False \ No newline at end of file