diff --git a/CHANGELOG.md b/CHANGELOG.md index 8bcf4814a7..8c63d84f08 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -36,6 +36,7 @@ Copy and pasting the git commit messages is __NOT__ enough. ### Fixed - Fixed a secondary exception that the `SumoTrafficSimulation` will throw when attempting to close a TraCI connection that is closed by an error. ### Removed +- Removed `vehicle_type` from `AgentInterface`, instead get the vehicle type from scenario's agent missions. ### Security ## [0.5.0] - 2022-01-07 diff --git a/smarts/core/agent_interface.py b/smarts/core/agent_interface.py index bab91ee0a4..114922556a 100644 --- a/smarts/core/agent_interface.py +++ b/smarts/core/agent_interface.py @@ -266,11 +266,6 @@ class AgentInterface: The choice of action space, this action space also decides the controller that will be enabled. """ - vehicle_type: str = "sedan" - """ - The choice of vehicle type. - """ - accelerometer: Union[Accelerometer, bool] = True """ Enable acceleration and jerk observations. @@ -293,7 +288,6 @@ def __post_init__(self): self.accelerometer = AgentInterface._resolve_config( self.accelerometer, Accelerometer ) - assert self.vehicle_type in {"sedan", "bus"} @staticmethod def from_type(requested_type: AgentType, **kwargs): diff --git a/smarts/core/controllers/__init__.py b/smarts/core/controllers/__init__.py index 33b8ec35c1..88837f949d 100644 --- a/smarts/core/controllers/__init__.py +++ b/smarts/core/controllers/__init__.py @@ -66,7 +66,6 @@ def perform_action( controller_state, sensor_state, action_space, - vehicle_type, ): """Calls control for the given vehicle based on a given action space and action. Args: @@ -84,13 +83,9 @@ def perform_action( The state of a vehicle sensor as relates to vehicle sensors. action_space: The action space of the provided action. - vehicle_type: - Vehicle type information about the given vehicle. """ if action is None: return - if vehicle_type == "bus": - assert action_space == ActionSpaceType.Trajectory if action_space == ActionSpaceType.Continuous: vehicle.control( throttle=np.clip(action[0], 0.0, 1.0), diff --git a/smarts/core/plan.py b/smarts/core/plan.py index 3998d884bc..7b001dfab9 100644 --- a/smarts/core/plan.py +++ b/smarts/core/plan.py @@ -26,7 +26,7 @@ from dataclasses import dataclass, field from typing import Optional, Tuple, Union -from smarts.sstudio.types import EntryTactic, TrapEntryTactic +from smarts.sstudio.types import EntryTactic, TrapEntryTactic, VehicleSpec from .coordinates import Dimensions, Heading, Point, Pose, RefLinePoint from .road_map import RoadMap @@ -185,16 +185,6 @@ class Via: hit_distance: float required_speed: float - -@dataclass(frozen=True) -class VehicleSpec: - """Vehicle specifications""" - - veh_id: str - veh_config_type: str - dimensions: Dimensions - - @dataclass(frozen=True) class Mission: """A navigation mission.""" diff --git a/smarts/core/scenario.py b/smarts/core/scenario.py index 21c69b8725..f15546871f 100644 --- a/smarts/core/scenario.py +++ b/smarts/core/scenario.py @@ -40,7 +40,6 @@ PositionalGoal, Start, TraverseGoal, - VehicleSpec, Via, default_entry_tactic, ) @@ -52,6 +51,7 @@ from smarts.sstudio import types as sstudio_types from smarts.sstudio.types import MapSpec from smarts.sstudio.types import Via as SSVia +from smarts.sstudio.types import VehicleSpec class Scenario: @@ -689,7 +689,6 @@ def to_scenario_via( road_map, ) goal = PositionalGoal(position, radius=2) - return Mission( start=start, route_vias=mission.route.via, @@ -697,6 +696,7 @@ def to_scenario_via( start_time=mission.start_time, entry_tactic=mission.entry_tactic, via=to_scenario_via(mission.via, road_map), + vehicle_spec=mission.vehicle_spec, ) elif isinstance(mission, sstudio_types.EndlessMission): position, heading = to_position_and_heading( @@ -711,6 +711,7 @@ def to_scenario_via( start_time=mission.start_time, entry_tactic=mission.entry_tactic, via=to_scenario_via(mission.via, road_map), + vehicle_spec=mission.vehicle_spec, ) elif isinstance(mission, sstudio_types.LapMission): start_road_id, start_lane, start_road_offset = mission.route.begin diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index 36e4270161..afa527eb26 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -573,7 +573,7 @@ def create_vehicle_in_providers( provider.create_vehicle( VehicleState( vehicle_id=vehicle.id, - vehicle_config_type="passenger", + vehicle_config_type=vehicle._vehicle_config_type, pose=vehicle.pose, dimensions=vehicle.chassis.dimensions, speed=vehicle.speed, @@ -928,7 +928,7 @@ def _get_provider_state(self, source: str, action_space_pred) -> ProviderState: provider_state.vehicles.append( VehicleState( vehicle_id=vehicle.id, - vehicle_config_type="passenger", + vehicle_config_type=vehicle._vehicle_config_type, pose=vehicle.pose, dimensions=vehicle.chassis.dimensions, speed=vehicle.speed, @@ -1239,7 +1239,6 @@ def _perform_agent_actions(self, agent_actions): controller_state, sensor_state, agent_interface.action_space, - agent_interface.vehicle_type, ) def _sync_vehicles_to_renderer(self): @@ -1354,10 +1353,15 @@ def _try_emit_envision_state(self, provider_state: ProviderState, obs, scores): for paths in vehicle_obs.road_waypoints.lanes.values() for path in paths ] + + veh_type = ( + v.vehicle_config_type if v.vehicle_config_type else v.vehicle_type + ) + traffic[v.vehicle_id] = envision_types.TrafficActorState( name=self._agent_manager.agent_name(agent_id), actor_type=actor_type, - vehicle_type=envision_types.VehicleType.Car, + vehicle_type=veh_type, position=tuple(v.pose.position), heading=float(v.pose.heading), speed=v.speed, diff --git a/smarts/core/vehicle.py b/smarts/core/vehicle.py index df29ad011c..fbc9fb177f 100644 --- a/smarts/core/vehicle.py +++ b/smarts/core/vehicle.py @@ -394,9 +394,16 @@ def build_agent_vehicle( SceneColors.Agent.value if trainable else SceneColors.SocialAgent.value ) - if agent_interface.vehicle_type == "sedan": + if mission.vehicle_spec == None: + vehicle_type = "passenger" + controller_type = "sedan" + else: + vehicle_type = mission.vehicle_spec.veh_config_type + controller_type = vehicle_type + + if vehicle_type == "passenger": urdf_name = "vehicle" - elif agent_interface.vehicle_type == "bus": + elif vehicle_type == "bus": urdf_name = "bus" else: raise Exception("Vehicle type does not exist!!!") @@ -406,7 +413,7 @@ def build_agent_vehicle( vehicle_filepath = str(path.absolute()) controller_parameters = sim.vehicle_index.controller_params_for_vehicle_type( - agent_interface.vehicle_type + controller_type ) chassis = None @@ -438,6 +445,7 @@ def build_agent_vehicle( id=vehicle_id, chassis=chassis, color=vehicle_color, + vehicle_config_type=vehicle_type ) return vehicle diff --git a/smarts/sstudio/types.py b/smarts/sstudio/types.py index 9c11d73c07..e0afdecaa5 100644 --- a/smarts/sstudio/types.py +++ b/smarts/sstudio/types.py @@ -39,7 +39,7 @@ from shapely.ops import split, unary_union from smarts.core import gen_id -from smarts.core.coordinates import RefLinePoint +from smarts.core.coordinates import RefLinePoint, Dimensions from smarts.core.default_map_builder import get_road_map from smarts.core.road_map import RoadMap from smarts.core.utils.id import SocialAgentId @@ -464,6 +464,14 @@ class TrapEntryTactic(EntryTactic): """The speed that the vehicle starts at when the hijack limit expiry emits a new vehicle""" +@dataclass(frozen=True) +class VehicleSpec: + """Vehicle specifications""" + + veh_id: str + veh_config_type: str + dimensions: Dimensions + @dataclass(frozen=True) class Mission: """The descriptor for an actor's mission.""" @@ -482,6 +490,9 @@ class Mission: entry_tactic: Optional[EntryTactic] = None """A specific tactic the mission should employ to start the mission.""" + vehicle_spec: Optional[VehicleSpec] = None + """Vehicle Specifications""" + @dataclass(frozen=True) class EndlessMission: @@ -503,6 +514,8 @@ class EndlessMission: """The earliest simulation time that this mission starts""" entry_tactic: Optional[EntryTactic] = None """A specific tactic the mission should employ to start the mission""" + vehicle_spec: Optional[VehicleSpec] = None + """Vehicle Specifications""" @dataclass(frozen=True) @@ -521,6 +534,8 @@ class LapMission: """The earliest simulation time that this mission starts""" entry_tactic: Optional[EntryTactic] = None """A specific tactic the mission should employ to start the mission""" + vehicle_spec: Optional[VehicleSpec] = None + """Vehicle Specifications""" @dataclass(frozen=True)