diff --git a/.github/workflows/ci-base-tests-linux.yml b/.github/workflows/ci-base-tests-linux.yml index 4b81b68c64..e81a3020e2 100644 --- a/.github/workflows/ci-base-tests-linux.yml +++ b/.github/workflows/ci-base-tests-linux.yml @@ -20,6 +20,7 @@ jobs: - ./smarts/env/tests/test_rllib_hiway_env.py - ./smarts/sstudio - ./examples/tests --ignore=./examples/tests/test_learning.py + - ./smarts/ray steps: - name: Checkout uses: actions/checkout@v2 @@ -29,7 +30,9 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install -e .[camera_obs,opendrive,rllib,test,test_notebook,torch,train,gym,argoverse] + pip install -e .[camera_obs,opendrive,test,test_notebook,torch,train,gym,argoverse] + if echo ${{matrix.tests}} | grep -q -e "env" -e "examples"; then pip install -e .[rllib]; fi + if echo ${{matrix.tests}} | grep -q "/ray"; then pip install -e .[ray]; fi - name: Run smoke tests run: | . ${{env.venv_dir}}/bin/activate diff --git a/.github/workflows/ci-base-tests-mac.yml b/.github/workflows/ci-base-tests-mac.yml index 11d4cf7343..144cebc5be 100644 --- a/.github/workflows/ci-base-tests-mac.yml +++ b/.github/workflows/ci-base-tests-mac.yml @@ -23,6 +23,7 @@ jobs: - ./smarts/env/tests/test_rllib_hiway_env.py - ./smarts/core --nb-exec-timeout 65536 --ignore=./smarts/core/tests/test_notebook.py - ./smarts/env --ignore=./smarts/env/tests/test_rllib_hiway_env.py + - ./smarts/ray steps: - name: Checkout uses: actions/checkout@v2 @@ -48,6 +49,8 @@ jobs: pip install wheel==0.38.4 pip install -r utils/setup/mac_requirements.txt pip install -e .[camera_obs,opendrive,rllib,test,test_notebook,torch,train,argoverse] + if echo ${{matrix.tests}} | grep -q -e "/env" -e "/examples"; then pip install -e .[rllib]; fi + if echo ${{matrix.tests}} | grep -q "/ray"; then pip install -e .[ray]; fi - name: Run smoke tests run: | . ${{env.venv_dir}}/bin/activate diff --git a/CHANGELOG.md b/CHANGELOG.md index 746ae3b523..a9d75f14af 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,10 +17,27 @@ Copy and pasting the git commit messages is __NOT__ enough. - Added baseline example, consisting of training, inference, and zoo agent registration, for the platooning task in Driving SMARTS 2023.3 benchmark. - Documented the challenge objective, desired inference code structure, and use of baseline example, for Driving SMARTS 2023.3 benchmark, i.e., platooning task. - Added a new scenario consisting of merge-exit map, sumo lead vehicle, and traffic, for the vehicle-following task. +- Added a `SensorManager` which manages placing sensors on actors in the simulations. +- The `VehicleState` now has the `bounding_box_points` property to get the vehicle minimum bounding box as a set of points. +- Added engine configuration options for `core:debug`, `core:observation_workers`, and `core:reset_retries`. - Explained in the docs that agents may spawn at different times in multiagent scenarios. +- Added `RaySensorResolver` as an alternative parallel resolver. +- Added `[ray]` option for `smarts` package. This currently conflicts with `[rllib]`. +- Added engine `observation_workers` configuration which can be used to configure the number of parallel sensor workers: 0 runs the sensors on the local thread, >=1 runs using the multiprocessing backing. +- Added engine `sensor_parallelization` configuration of sensor parallelization backing, options ("mp"|"ray"): "mp" python multiprocessing, "ray" ray worker backing. +- Added engine `reset_retries` configuration engine retries before the simulator will raise an error on reset. ### Changed - The trap manager, `TrapManager`, is now a subclass of `ActorCaptureManager`. - Considering lane-change time ranges between 3s and 6s, assuming a speed of 13.89m/s, the via sensor lane acquisition range was increased from 40m to 80m, for better driving ability. +- The `AgentType.Full` now includes `road_waypoints`, `accelerometer`, and `lane_positions`. +- `ActionSpaceType` has been moved from `controller` to its own file. +- `VehicleState` has been moved to its own file. +- Sensors are no longer added and configured in the `agent_manager`. They are instead managed in the `sensor_manager`. +- Renamed all terminology relating to actor to owner in `VehicleIndex`. +- Renamed all terminology relating to shadow actor to shadower in `VehicleIndex`. +- `Collision` has been moved from `smarts.core.observations` to `smarts.core.vehicle_state`. +- The trap manager, `TrapManager`, is now a subclass of `ActorCaptureManager`. +- Considering lane-change time ranges between 3s and 6s, assuming a speed of 13.89m/s, the via sensor lane acquisition range was increased from 40m to 80m, for better driving ability. - Modified naming of benchmark used in NeurIPS 2022 from driving-smarts-competition-env to driving-smarts-v2022. - Sstudio generated scenario vehicle traffic ids are now shortened. ### Deprecated @@ -33,6 +50,7 @@ Copy and pasting the git commit messages is __NOT__ enough. - Fixed issues related to waypoints in SUMO maps. Waypoints in junctions should now return all possible paths through the junction. ### Removed - Removed the deprecated `waymo_browser` utility. +- Removed camera observation `created_at` attribute from metadata to make observation completely reproducible. ### Security ## [1.0.11] # 2023-04-02 diff --git a/Makefile b/Makefile index 3ecbd6dfe6..c742e8740c 100644 --- a/Makefile +++ b/Makefile @@ -43,6 +43,13 @@ sanity-test: build-sanity-scenarios test-learning: build-all-scenarios pytest -v -s -o log_cli=1 -o log_cli_level=INFO ./examples/tests/test_learning.py +.PHONY: test-long-determinism +test-long-determinism: + scl scenario build --clean scenarios/sumo/minicity + PYTHONHASHSEED=42 pytest -v \ + --forked \ + ./smarts/env/tests/test_determinism.py::test_long_determinism + .PHONY: test-memory-growth test-memory-growth: build-all-scenarios PYTHONHASHSEED=42 pytest -v \ @@ -54,13 +61,6 @@ test-memory-growth: build-all-scenarios rm -f .coverage.* rm -f .coverage* -.PHONY: test-long-determinism -test-long-determinism: - scl scenario build --clean scenarios/sumo/minicity - PYTHONHASHSEED=42 pytest -v \ - --forked \ - ./smarts/env/tests/test_determinism.py::test_long_determinism - .PHONY: benchmark benchmark: build-all-scenarios pytest -v ./smarts/env/tests/test_benchmark.py @@ -143,7 +143,7 @@ clean: rm -f .coverage* .PHONY: format -format: +format: gen-header echo "isort, version `isort --version-number`" isort -m VERTICAL_HANGING_INDENT --skip-gitignore --ac --tc --profile black ./baselines ./cli ./envision ./examples/ ./utils/ ./scenarios/ ./smarts ./zoo black --version diff --git a/docs/sim/agent.rst b/docs/sim/agent.rst index 43da9963d7..61a54f17f4 100644 --- a/docs/sim/agent.rst +++ b/docs/sim/agent.rst @@ -156,7 +156,7 @@ A pre-configured `interface` can be extended by supplying extra `kwargs`. For ex agent_interface = AgentInterface.from_type( requested_type = AgentType.Standard, - lidar = True, + lidar_point_cloud = True, ) Custom interface diff --git a/examples/tools/pybullet_sumo_orientation_example.py b/examples/tools/pybullet_sumo_orientation_example.py index 6c3aca0e2c..b22f9e7611 100644 --- a/examples/tools/pybullet_sumo_orientation_example.py +++ b/examples/tools/pybullet_sumo_orientation_example.py @@ -4,14 +4,15 @@ import numpy as np -from smarts.core.actor_role import ActorRole +from smarts.core.actor import ActorRole from smarts.core.chassis import BoxChassis from smarts.core.coordinates import Heading, Pose from smarts.core.scenario import Scenario from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation from smarts.core.utils import pybullet from smarts.core.utils.pybullet import bullet_client as bc -from smarts.core.vehicle import VEHICLE_CONFIGS, Vehicle, VehicleState +from smarts.core.vehicle import VEHICLE_CONFIGS, Vehicle +from smarts.core.vehicle_state import VehicleState TIMESTEP_SEC = 1 / 240 INITIAL_KINEMATICS_VEHICLES = 50 diff --git a/examples/tools/pybullet_vehicle_example.py b/examples/tools/pybullet_vehicle_example.py index 3cd1197450..5b6fa2e9ad 100644 --- a/examples/tools/pybullet_vehicle_example.py +++ b/examples/tools/pybullet_vehicle_example.py @@ -31,6 +31,7 @@ def look_at(client, position=(0, 0, 0), top_down=True): ) +# pytype: disable=name-error def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): prev_friction_sum = None @@ -50,9 +51,8 @@ def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): ) client.stepSimulation() - vehicle.sync_to_renderer() - frictions_ = frictions(sliders) + frictions_ = frictions(sliders, client) if prev_friction_sum is not None and not math.isclose( sum(frictions_.values()), prev_friction_sum @@ -74,7 +74,10 @@ def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): ) -def frictions(sliders): +# pytype: enable=name-error + + +def frictions(sliders, client): return dict( lateralFriction=client.readUserDebugParameter(sliders["lateral_friction"]), spinningFriction=client.readUserDebugParameter(sliders["spinning_friction"]), @@ -128,7 +131,7 @@ def frictions(sliders): path = str(path.absolute()) plane_body_id = client.loadURDF(path, useFixedBase=True) - client.changeDynamics(plane_body_id, -1, **frictions(sliders)) + client.changeDynamics(plane_body_id, -1, **frictions(sliders, client)) pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( diff --git a/setup.cfg b/setup.cfg index 1ed6a48b77..d134b719b2 100644 --- a/setup.cfg +++ b/setup.cfg @@ -94,6 +94,8 @@ rllib = opencv-python==4.1.2.30 opencv-python-headless==4.1.2.30 ray[rllib]==1.4.0 +ray = + ray~=2.2.0 ros = catkin_pkg rospkg diff --git a/smarts/core/actor.py b/smarts/core/actor.py index c993f117b1..2b05627f4f 100644 --- a/smarts/core/actor.py +++ b/smarts/core/actor.py @@ -63,4 +63,6 @@ def __hash__(self) -> int: return hash(self.actor_id) def __eq__(self, other) -> bool: - return self.__class__ == other.__class__ and hash(self) == hash(other) + return self.__class__ == other.__class__ and hash(self.actor_id) == hash( + other.actor_id + ) diff --git a/smarts/core/actor_capture_manager.py b/smarts/core/actor_capture_manager.py index c378978956..c7d88cf44b 100644 --- a/smarts/core/actor_capture_manager.py +++ b/smarts/core/actor_capture_manager.py @@ -89,7 +89,7 @@ def __make_new_social_vehicle(sim, agent_id, initial_speed): social_agent_spec, social_agent_model, ) - vehicles = sim.vehicle_index.vehicles_by_actor_id(agent_id) + vehicles = sim.vehicle_index.vehicles_by_owner_id(agent_id) return vehicles[0] if len(vehicles) else None diff --git a/smarts/core/agent_interface.py b/smarts/core/agent_interface.py index 2bdc7adb3a..d6f338951f 100644 --- a/smarts/core/agent_interface.py +++ b/smarts/core/agent_interface.py @@ -22,7 +22,7 @@ from enum import IntEnum from typing import List, Optional, Tuple, Union -from smarts.core.controllers import ActionSpaceType +from smarts.core.controllers.action_space_type import ActionSpaceType from smarts.core.lidar_sensor_params import BasicLidar from smarts.core.lidar_sensor_params import SensorParams as LidarSensorParams @@ -367,10 +367,13 @@ def from_type(requested_type: AgentType, **kwargs): interface = AgentInterface( neighborhood_vehicle_states=True, waypoint_paths=True, + road_waypoints=True, drivable_area_grid_map=True, occupancy_grid_map=True, top_down_rgb=True, lidar_point_cloud=True, + accelerometer=True, + lane_positions=True, signals=True, action=ActionSpaceType.Continuous, ) diff --git a/smarts/core/agent_manager.py b/smarts/core/agent_manager.py index ba05996ec2..e363ccc99e 100644 --- a/smarts/core/agent_manager.py +++ b/smarts/core/agent_manager.py @@ -21,7 +21,7 @@ import logging import weakref from concurrent import futures -from typing import Any, Callable, Dict, Optional, Set, Tuple, Union +from typing import Any, Callable, Dict, List, Optional, Set, Tuple, Union from envision.types import format_actor_id from smarts.core.actor import ActorRole @@ -31,9 +31,10 @@ from smarts.core.heterogenous_agent_buffer import HeterogenousAgentBuffer from smarts.core.observations import Observation from smarts.core.plan import Mission, Plan, PositionalGoal +from smarts.core.sensor_manager import SensorManager from smarts.core.sensors import Sensors from smarts.core.utils.id import SocialAgentId -from smarts.core.vehicle import VehicleState +from smarts.core.vehicle_state import VehicleState from smarts.zoo.registry import make as make_social_agent @@ -46,14 +47,16 @@ class AgentManager: """ def __init__(self, sim, interfaces, zoo_addrs=None): + from smarts.core.vehicle_index import VehicleIndex + self._log = logging.getLogger(self.__class__.__name__) self._sim = weakref.ref(sim) - self._vehicle_index = sim.vehicle_index + self._vehicle_index: VehicleIndex = sim.vehicle_index + self._sensor_manager: SensorManager = sim.sensor_manager self._agent_buffer = None self._zoo_addrs = zoo_addrs self._ego_agent_ids = set() self._social_agent_ids = set() - self._vehicle_with_sensors_to_agent = dict() # Initial interfaces are for agents that are spawned at the beginning of the # episode and that we'd re-spawn upon episode reset. This would include ego @@ -81,7 +84,6 @@ def teardown(self): self._log.debug("Tearing down AgentManager") self.teardown_ego_agents() self.teardown_social_agents() - self._vehicle_with_sensors_to_agent = dict() self._pending_agent_ids = set() def destroy(self): @@ -129,6 +131,11 @@ def active_agents(self) -> Set[str]: """A list of all active agents in the simulation (agents that have a vehicle.)""" return self.agent_ids - self.pending_agent_ids + @property + def shadowing_agent_ids(self) -> Set[str]: + """Get all agents that currently observe, but not control, a vehicle.""" + return self._vehicle_index.shadower_ids() + def is_ego(self, agent_id) -> bool: """Test if the agent is an ego agent.""" return agent_id in self.ego_agent_ids @@ -138,6 +145,26 @@ def remove_pending_agent_ids(self, agent_ids): assert agent_ids.issubset(self.agent_ids) self._pending_agent_ids -= agent_ids + def agent_for_vehicle(self, vehicle_id) -> str: + """Get the controlling agent for the given vehicle.""" + return self._vehicle_index.owner_id_from_vehicle_id(vehicle_id) + + def agent_has_vehicle(self, agent_id) -> bool: + """Test if an agent has an actor associated with it.""" + return len(self.vehicles_for_agent(agent_id)) > 0 + + def vehicles_for_agent(self, agent_id) -> List[str]: + """Get the vehicles associated with an agent.""" + return self._vehicle_index.vehicle_ids_by_owner_id( + agent_id, include_shadowers=True + ) + + def _vehicle_has_agent(self, a_id, v_or_v_id): + assert ( + a_id + ), f"Vehicle `{getattr(v_or_v_id, 'id', v_or_v_id)}` does not have an agent registered to it to get observations for." + return v_or_v_id is not None and a_id is not None + def observe_from( self, vehicle_ids: Set[str], done_this_step: Optional[Set[str]] = None ) -> Tuple[ @@ -151,31 +178,33 @@ def observe_from( rewards = {} dones = {} scores = {} - for v_id in vehicle_ids: - vehicle = self._vehicle_index.vehicle_by_id(v_id, None) - if not vehicle: - self._log.warning( - "Attempted to observe non-existing vehicle `%s`", v_id - ) - agent_id = self.vehicle_with_sensors_to_agent(v_id) - if not agent_id: - continue - - if not self._vehicle_index.check_vehicle_id_has_sensor_state(vehicle.id): - continue - - sensor_state = self._vehicle_index.sensor_state_for_vehicle_id(vehicle.id) - - observations[agent_id], dones[agent_id] = Sensors.observe( - sim, agent_id, sensor_state, vehicle - ) - rewards[agent_id] = vehicle.trip_meter_sensor(increment=True) - scores[agent_id] = vehicle.trip_meter_sensor() + sim_frame = sim.cached_frame + agent_vehicle_pairs = { + self.agent_for_vehicle(v_id): v_id for v_id in vehicle_ids + } + agent_ids = { + a_id + for a_id, v_id in agent_vehicle_pairs.items() + if self._vehicle_has_agent(a_id, v_id) + } + observations, dones = sim.sensor_manager.observe( + sim_frame, + sim.local_constants, + agent_ids, + sim.renderer_ref, + sim.bc, + ) + for agent_id in agent_ids: + v_id = agent_vehicle_pairs[agent_id] + sensors = sim.sensor_manager.sensors_for_actor_ids([v_id]) + trip_meter_sensor = sensors[v_id]["trip_meter_sensor"] + rewards[agent_id] = trip_meter_sensor(increment=True) + scores[agent_id] = trip_meter_sensor() # also add agents that were done in virtue of just dropping out for done_v_id in done_this_step: - agent_id = self._vehicle_with_sensors_to_agent.get(done_v_id, None) + agent_id = self._vehicle_index.owner_id_from_vehicle_id(done_v_id) if agent_id: dones[agent_id] = True @@ -196,68 +225,91 @@ def observe( rewards = {} scores = {} dones = { - agent_id: agent_id not in self.pending_agent_ids + agent_id: agent_id not in self.pending_agent_ids | self.shadowing_agent_ids for agent_id in self.agent_ids - if agent_id not in self._vehicle_index.agent_vehicle_ids() + if not self.agent_has_vehicle(agent_id) } - for agent_id in self.active_agents: - # An agent may be pointing to its own vehicle or observing a social vehicle - vehicle_ids = self._vehicle_index.vehicle_ids_by_actor_id( - agent_id, include_shadowers=True - ) + sim_frame = sim.cached_frame + active_standard_agents = set() + active_boid_agents = set() + for agent_id in self.active_agents: if self.is_boid_agent(agent_id): - vehicles = [ - self._vehicle_index.vehicle_by_id(vehicle_id) - for vehicle_id in vehicle_ids - ] - # returns format of {: {: {...}}} - sensor_states = { - vehicle.id: self._vehicle_index.sensor_state_for_vehicle_id( - vehicle.id - ) - for vehicle in vehicles - } - observations[agent_id], dones[agent_id] = Sensors.observe_batch( - sim, agent_id, sensor_states, {v.id: v for v in vehicles} - ) - rewards[agent_id] = { - vehicle_id: self._vehicle_reward(vehicle_id) - for vehicle_id in sensor_states.keys() - } - scores[agent_id] = { - format_actor_id( - agent_id, vehicle_id, is_multi=True - ): self._vehicle_score(vehicle_id) - for vehicle_id in sensor_states.keys() - } + active_boid_agents.add(agent_id) else: - self._diagnose_mismatched_observation_vehicles(vehicle_ids, agent_id) - - vehicle = self._vehicle_index.vehicle_by_id(vehicle_ids[0]) - sensor_state = self._vehicle_index.sensor_state_for_vehicle_id( - vehicle.id - ) - obs, dones[agent_id] = Sensors.observe( - sim, agent_id, sensor_state, vehicle - ) - observations[agent_id] = obs + active_standard_agents.add(agent_id) - if self._vehicle_index.vehicle_is_shadowed(vehicle.id): - # It is not a shadowing agent's fault if it is done - dones[agent_id] = False - else: - logging.log( - logging.DEBUG, - f"Agent `{agent_id}` has raised done with {obs.events}", - ) - - rewards[agent_id] = vehicle.trip_meter_sensor(increment=True) - scores[agent_id] = vehicle.trip_meter_sensor() + agent_vehicle_pairs = { + a_id: (self.vehicles_for_agent(a_id) + [None])[0] + for a_id in active_standard_agents + } + agent_ids = { + a_id + for a_id, v in agent_vehicle_pairs.items() + if self._vehicle_has_agent(a_id, v) + and self._sensor_manager.sensor_state_exists(v) + } + observations, new_dones = sim.sensor_manager.observe( + sim_frame, + sim.local_constants, + agent_ids, + sim.renderer_ref, + sim.bc, + ) + dones.update(new_dones) + for agent_id in agent_ids: + v_id = agent_vehicle_pairs[agent_id] + sensors = sim.sensor_manager.sensors_for_actor_ids([v_id]) + trip_meter_sensor = sensors[v_id]["trip_meter_sensor"] + rewards[agent_id] = trip_meter_sensor(increment=True) + scores[agent_id] = trip_meter_sensor() + + ## TODO MTA, support boid agents with parallel observations + for agent_id in active_boid_agents: + # An agent may be pointing to its own vehicle or observing a social vehicle + vehicle_ids = self._vehicle_index.vehicle_ids_by_owner_id( + agent_id, include_shadowers=True + ) + vehicles = [ + self._vehicle_index.vehicle_by_id(vehicle_id) + for vehicle_id in vehicle_ids + ] + # returns format of {: {: {...}}} + sensor_states = { + vehicle.id: self._sensor_manager.sensor_state_for_actor_id(vehicle.id) + for vehicle in vehicles + } + observations[agent_id], dones[agent_id] = sim.sensor_manager.observe_batch( + sim_frame, + sim.local_constants, + agent_id, + sensor_states, + {v.id: v for v in vehicles}, + sim.renderer_ref, + sim.bc, + ) + # TODO: Observations and rewards should not be generated here. + rewards[agent_id] = { + vehicle_id: self._vehicle_reward(vehicle_id) + for vehicle_id in sensor_states.keys() + } + scores[agent_id] = { + format_actor_id( + agent_id, vehicle_id, is_multi=True + ): self._vehicle_score(vehicle_id) + for vehicle_id in sensor_states.keys() + } if sim.should_reset: - dones = {agent_id: True for agent_id in self.agent_ids} + for agent_id in dones: + if self.is_boid_agent(agent_id): + for v_id in dones[agent_id]: + # pytype: disable=unsupported-operands + dones[agent_id][v_id] = True + # pytype: enable=unsupported-operands + else: + dones[agent_id] = True dones["__sim__"] = True return observations, rewards, scores, dones @@ -289,16 +341,6 @@ def _vehicle_reward(self, vehicle_id: str) -> float: def _vehicle_score(self, vehicle_id: str) -> float: return self._vehicle_index.vehicle_by_id(vehicle_id).trip_meter_sensor() - def step_sensors(self): - """Update all known vehicle sensors.""" - # TODO: Move to vehicle index - for vehicle_id, sensor_state in self._vehicle_index.sensor_states_items(): - Sensors.step(self, sensor_state) - - vehicle = self._vehicle_index.vehicle_by_id(vehicle_id) - for sensor in vehicle.sensors.values(): - sensor.step() - def _filter_for_active_ego(self, dict_): return { id_: dict_[id_] @@ -363,7 +405,7 @@ def _filter_social_agent_actions_for_controlled_vehicles( vehicle_ids_controlled_by_agents = self._vehicle_index.agent_vehicle_ids() controlling_agent_ids = set( [ - self._vehicle_index.actor_id_from_vehicle_id(v_id) + self._vehicle_index.owner_id_from_vehicle_id(v_id) for v_id in vehicle_ids_controlled_by_agents ] ) @@ -377,7 +419,7 @@ def _filter_social_agent_actions_for_controlled_vehicles( # Handle boids where some vehicles are hijacked and some have not yet been for agent_id, actions in social_agent_actions.items(): if self.is_boid_agent(agent_id): - controlled_vehicle_ids = self._vehicle_index.vehicle_ids_by_actor_id( + controlled_vehicle_ids = self._vehicle_index.vehicle_ids_by_owner_id( agent_id, include_shadowers=False ) social_agent_actions[agent_id] = { @@ -676,63 +718,12 @@ def is_boid_keep_alive_agent(self, agent_id: str) -> bool: return self._social_agent_data_models[agent_id].is_boid_keep_alive - def vehicle_with_sensors_to_agent(self, vehicle_id: str) -> Optional[str]: - """Maps a vehicle to an agent if the vehicle has sensors. - - Args: - vehicle_id (str): The vehicle to check for an agent. - - Returns: - Optional[str]: - The agent id if the vehicle has a sensor. `None` if the vehicle does - not exist or is not mapped to an agent. - """ - if not vehicle_id in self._vehicle_with_sensors_to_agent: - return None - if not self._vehicle_index.vehicle_by_id(vehicle_id, None): - del self._vehicle_with_sensors_to_agent[vehicle_id] - return None - return self._vehicle_with_sensors_to_agent[vehicle_id] - - def attach_sensors_to_vehicles(self, agent_interface, vehicle_ids): - """Attach the interface required sensors to the given vehicles""" - sim = self._sim() - assert sim - for sv_id in vehicle_ids: - if not self._vehicle_index.vehicle_by_id(sv_id, None): - self._log.warning( - "Attempted to add sensors to non-existing vehicle: %s.", sv_id - ) - continue - - if self.vehicle_with_sensors_to_agent(sv_id): - continue - - # If this is a history vehicle, assign a mission based on its final position. - # This is necessary so that the observations have waypoints that lead to the goal. - mission = None - if sv_id in sim.traffic_history_provider.history_vehicle_ids: - v_id = sv_id.split("-")[-1] - start_time = sim.scenario.traffic_history.vehicle_initial_time(v_id) - start, _ = sim.scenario.get_vehicle_start_at_time(v_id, start_time) - veh_goal = sim.scenario.get_vehicle_goal(v_id) - mission = Mission( - start=start, - goal=PositionalGoal(veh_goal, radius=5), - ) - plan = Plan(sim.road_map, mission) - - agent_id = f"Agent-{sv_id}" - self._vehicle_with_sensors_to_agent[sv_id] = agent_id - self._agent_interfaces[agent_id] = agent_interface + def is_boid_done(self, agent_id: str) -> bool: + """Check if this boid agent should not disappear yet.""" + if self.is_boid_keep_alive_agent(agent_id): + return False - self._vehicle_index.attach_sensors_to_vehicle( - sim, sv_id, agent_interface, plan - ) + if self.agent_has_vehicle(agent_id): + return False - def detach_sensors_from_vehicle(self, vehicle_id: str): - """Called when agent observation is finished and sensors should be removed from a vehicle""" - if not self.vehicle_with_sensors_to_agent(vehicle_id): - return - self._vehicle_index.stop_agent_observation(vehicle_id) - del self._vehicle_with_sensors_to_agent[vehicle_id] + return True diff --git a/smarts/core/agents_provider.py b/smarts/core/agents_provider.py index 67ad83a8dd..2c3d0186c7 100644 --- a/smarts/core/agents_provider.py +++ b/smarts/core/agents_provider.py @@ -28,7 +28,7 @@ from .controllers import ActionSpaceType, Controllers from .provider import Provider, ProviderManager, ProviderRecoveryFlags, ProviderState from .road_map import RoadMap -from .vehicle import VehicleState +from .vehicle_state import VehicleState class AgentsProvider(Provider): @@ -71,6 +71,15 @@ def _agent_manager(self): return sim.agent_manager # pytype: enable=attribute-error + @property + def _sensor_manager(self): + sim = self._sim() + assert sim + # pytype: disable=attribute-error + # TAI: consider adding to ProviderManager interface + return sim.sensor_manager + # pytype: enable=attribute-error + @property def actions(self) -> Set[ActionSpaceType]: # must be implemented by derived classes @@ -132,6 +141,7 @@ def perform_agent_actions(self, agent_actions: Dict[str, Any]): ) agent_manager = self._agent_manager + sensor_manager = self._sensor_manager vehicle_index = self._vehicle_index sim = self._sim() assert sim @@ -150,7 +160,7 @@ def perform_agent_actions(self, agent_actions: Dict[str, Any]): controller_state = vehicle_index.controller_state_for_vehicle_id( vehicle.id ) - sensor_state = vehicle_index.sensor_state_for_vehicle_id(vehicle.id) + sensor_state = sensor_manager.sensor_state_for_actor_id(vehicle.id) Controllers.perform_action( sim, agent_id, @@ -189,7 +199,7 @@ def add_actor( self, provider_actor: ActorState, from_provider: Optional[Provider] = None ): provider_actor.source = self.source_str - agent_id = self._vehicle_index.actor_id_from_vehicle_id(provider_actor.actor_id) + agent_id = self._vehicle_index.owner_id_from_vehicle_id(provider_actor.actor_id) self._my_agent_actors.setdefault(agent_id, []).append(provider_actor) def _agent_for_vehicle(self, vehicle_id: str) -> Optional[Tuple[str, int]]: diff --git a/smarts/core/bubble_manager.py b/smarts/core/bubble_manager.py index 188e715a30..6a4279e16e 100644 --- a/smarts/core/bubble_manager.py +++ b/smarts/core/bubble_manager.py @@ -444,7 +444,7 @@ def is_active(bubble): vehicles = [] if bubble.follow_actor_id is not None: - vehicles += self._last_vehicle_index.vehicles_by_actor_id( + vehicles += self._last_vehicle_index.vehicles_by_owner_id( bubble.follow_actor_id ) if bubble.follow_vehicle_id is not None: @@ -497,7 +497,7 @@ def agent_ids_for_bubble(self, bubble: Bubble, sim) -> Set[str]: for bc in bubble_cursors: if bc.state != BubbleState.InBubble: continue - agent_id = sim.vehicle_index.actor_id_from_vehicle_id(bc.vehicle_id) + agent_id = sim.vehicle_index.owner_id_from_vehicle_id(bc.vehicle_id) if agent_id is not None: agent_ids.add(agent_id) return agent_ids @@ -579,7 +579,7 @@ def _sync_cursors(self, last_vehicle_index, vehicle_index): def _handle_transitions(self, sim, cursors: Set[Cursor]): social_agent_vehicles = [] for agent_id in sim.agent_manager.social_agent_ids: - social_agent_vehicles += sim.vehicle_index.vehicles_by_actor_id(agent_id) + social_agent_vehicles += sim.vehicle_index.vehicles_by_owner_id(agent_id) transitioned = [c for c in cursors if c.transition is not None] for cursor in transitioned: @@ -608,7 +608,7 @@ def _move_travelling_bubbles(self, sim): # XXX: Using a vehicle reference through the `_last_vehicle_index` is a # XXX clear error since it can reference out of date vehicles. if bubble.follow_actor_id is not None: - vehicles += self._last_vehicle_index.vehicles_by_actor_id( + vehicles += self._last_vehicle_index.vehicles_by_owner_id( bubble.follow_actor_id ) if bubble.follow_vehicle_id is not None: @@ -679,7 +679,6 @@ def _hijack_social_vehicle_with_social_agent( else: agent_id = BubbleManager._make_social_agent_id(vehicle_id) - # TODO MTA: Resolve potential issue if vehicle is not acquired agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id) vehicle = sim.vehicle_index.switch_control_to_agent( sim, @@ -690,12 +689,13 @@ def _hijack_social_vehicle_with_social_agent( recreate=False, agent_interface=agent_interface, ) - sim.create_vehicle_in_providers(vehicle, agent_id) + if vehicle: + sim.create_vehicle_in_providers(vehicle, agent_id) def _prepare_sensors_for_agent_control( self, sim, vehicle_id, agent_id, agent_interface, bubble ): - plan = Plan(sim.road_map, None, find_route=False) + plan = Plan(sim.road_map, None) vehicle = sim.vehicle_index.start_agent_observation( sim, vehicle_id, diff --git a/smarts/core/configuration.py b/smarts/core/configuration.py index 48ad81057a..ef781f8b90 100644 --- a/smarts/core/configuration.py +++ b/smarts/core/configuration.py @@ -30,7 +30,7 @@ def _convert_truthy(t: str) -> bool: - """Convert value to a boolean. This should only allow ([Tt]rue)|([Ff]alse)|[\d]. + """Convert value to a boolean. This should only allow ([Tt]rue)|([Ff]alse)|[\\d]. This is necessary because bool("false") == True. Args: @@ -72,6 +72,7 @@ def __init__( raise FileNotFoundError(f"Configuration file not found at {config_file}") self._config.read(str(config_file.absolute())) + print(f"Using configuration from: {config_file.absolute()}") @property def environment_prefix(self): diff --git a/smarts/core/controllers/__init__.py b/smarts/core/controllers/__init__.py index 91f890eb8b..0119988b66 100644 --- a/smarts/core/controllers/__init__.py +++ b/smarts/core/controllers/__init__.py @@ -43,105 +43,9 @@ TrajectoryTrackingControllerState, ) -METER_PER_SECOND_TO_KM_PER_HR = 3.6 - - -class ActionSpaceType(Enum): - """Available vehicle action spaces.""" - - Continuous = 0 - """ - Action=(throttle, brake, steering) +from .action_space_type import ActionSpaceType - + throttle: Range=[0, 1]. Type=float. - + brake: Range=[0, 1]. Type=float. - + steering: Range=[-1, 1]. Type=float. - - Steering maps to full turn in one direction to a full turn in the other. - Direction of turn for the steering depends on the vehicle. - """ - Lane = 1 - """ - Action= ``str``. Discrete lane action from one of - - + "keep_lane", - + "slow_down", - + "change_lane_left", and - + "change_lane_right". - """ - ActuatorDynamic = 2 - """ - Action=(throttle, brake, steering_rate) - - + throttle: Range=[0, 1]. Type=float. - + brake: Range=[0, 1]. Type=float. - + steering_rate: Range[-1, 1]. Type=float. - - Steering rate means the amount of steering change *per second*. - (either positive or negative) to be applied to the current steering. - This gets clipped to the available steering of the vehicle (which may vary.) - """ - LaneWithContinuousSpeed = 3 - """ - Action=(target_speed, lane_change). - - + target_speed: Baseline target speed (controller may give more or less - regardless). Type=float. - + lane_change: Discrete lane change value. Can be one of - - + -1 : change to right lane - + 0 : keep to current lane - + 1 : change to left lane - """ - TargetPose = 4 - """ - Action=(x_coord, y_coord, heading, time_delta). Type= ``Sequence[float, - float, float, float]``. Continuous action space of vehicle's next x - coordinate, y coordinate, heading, and time delta to reach the given pose. - """ - Trajectory = 5 - """ - Action=([x_coord],[y_coord],[heading],[speed]). Type= ``(Sequence[float], - Sequence[float], Sequence[float], Sequence[float])``. Continuous action - space using trajectory comprising of x coordinates, y coordinates, headings, - and speeds, to directly move a vehicle. - """ - MultiTargetPose = 6 # For boid control. - """ - Action= ``Dict[str, (float, float, float, float)]``. Continuous action space - that provides ``TargetPose`` actions for multiple vehicles. - """ - MPC = 7 - """ - Action=([x_coord],[y_coord],[heading],[speed]). Type= ``(Sequence[float], - Sequence[float], Sequence[float], Sequence[float])``. Adaptive control - performed on the vehicle model to match the given trajectory comprising - of vehicle's x coordinates, y coordinates, headings, and speeds. - """ - TrajectoryWithTime = 8 # For pure interpolation provider. - """ - Action=([time],[x_coord],[y_coord],[heading],[speed]). - Type= ``(Sequence[float], Sequence[float], Sequence[float], Sequence[float], - Sequence[float])``. Interpolates vehicle along the given trajectory - comprising of times, x coordinates, y coordinates, headings, and speeds. - """ - Direct = 9 - """ - Action=(speed) OR (acceleration, angular_velocity). Type= ``Union[float, - Tuple[float,float]]``. Continuous action space where we can pass either (a) an - initial speed upon reset, or (b) linear acceleration and angular velocity - for other steps. - """ - Empty = 10 - """ - Action=(). Type= ``Tuple[]. This action is empty.`` - """ - RelativeTargetPose = 11 - """ - Action=(delta_x, delta_y, delta_heading). Type= ``Tuple[float, - float, float]``. Continuous action space of vehicle's next pose in terms of delta x - coordinate, delta y coordinate, and delta heading, to be reached in 0.1 seconds. - """ +METER_PER_SECOND_TO_KM_PER_HR = 3.6 class Controllers: diff --git a/smarts/core/controllers/action_space_type.py b/smarts/core/controllers/action_space_type.py new file mode 100644 index 0000000000..0310862e2d --- /dev/null +++ b/smarts/core/controllers/action_space_type.py @@ -0,0 +1,116 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from enum import Enum + + +class ActionSpaceType(Enum): + """Available vehicle action spaces.""" + + Continuous = 0 + """ + Action=(throttle, brake, steering) + + throttle: Range=[0, 1]. Type=float. + + brake: Range=[0, 1]. Type=float. + + steering: Range=[-1, 1]. Type=float. + Steering maps to full turn in one direction to a full turn in the other. + Direction of turn for the steering depends on the vehicle. + """ + Lane = 1 + """ + Action= ``str``. Discrete lane action from one of + + + "keep_lane", + + "slow_down", + + "change_lane_left", and + + "change_lane_right". + """ + ActuatorDynamic = 2 + """ + Action=(throttle, brake, steering_rate) + + + throttle: Range=[0, 1]. Type=float. + + brake: Range=[0, 1]. Type=float. + + steering_rate: Range[-1, 1]. Type=float. + + Steering rate means the amount of steering change *per second*. + (either positive or negative) to be applied to the current steering. + This gets clipped to the available steering of the vehicle (which may vary.) + """ + LaneWithContinuousSpeed = 3 + """ + Action=(target_speed, lane_change). + + target_speed: Baseline target speed (controller may give more or less + regardless). Type=float. + + lane_change: Discrete lane change value. Can be one of + + -1 : change to right lane + + 0 : keep to current lane + + 1 : change to left lane + """ + TargetPose = 4 + """ + Action=(x_coord, y_coord, heading, time_delta). Type= ``Sequence[float, + float, float, float]``. Continuous action space of vehicle's next x + coordinate, y coordinate, heading, and time delta to reach the given pose. + """ + Trajectory = 5 + """ + Action=([x_coord],[y_coord],[heading],[speed]). Type= ``(Sequence[float], + Sequence[float], Sequence[float], Sequence[float])``. Continuous action + space using trajectory comprising of x coordinates, y coordinates, headings, + and speeds, to directly move a vehicle. + """ + MultiTargetPose = 6 # For boid control. + """ + Action= ``Dict[str, (float, float, float, float)]``. Continuous action space + that provides ``TargetPose`` actions for multiple vehicles. + """ + MPC = 7 + """ + Action=([x_coord],[y_coord],[heading],[speed]). Type= ``(Sequence[float], + Sequence[float], Sequence[float], Sequence[float])``. Adaptive control + performed on the vehicle model to match the given trajectory comprising + of vehicle's x coordinates, y coordinates, headings, and speeds. + """ + TrajectoryWithTime = 8 # For pure interpolation provider. + """ + Action=([time],[x_coord],[y_coord],[heading],[speed]). + Type= ``(Sequence[float], Sequence[float], Sequence[float], Sequence[float], + Sequence[float])``. Interpolates vehicle along the given trajectory + comprising of times, x coordinates, y coordinates, headings, and speeds. + """ + Direct = 9 + """ + Action=(speed) OR (acceleration, angular_velocity). Type= ``Union[float, + (float,float)]``. Continuous action space where we can pass either (a) an + initial speed upon reset, or (b) linear acceleration and angular velocity + for other steps. + """ + Empty = 10 + """ + Action=(). Type= ``Tuple[]. This action is empty.`` + """ + RelativeTargetPose = 11 + """ + Action=(delta_x, delta_y, delta_heading). Type= ``Sequence[float, + float, float]``. Continuous action space of vehicle's next pose in terms of delta x + coordinate, delta y coordinate, and delta heading, to be reached in 0.1 seconds. + """ diff --git a/smarts/core/controllers/actuator_dynamic_controller.py b/smarts/core/controllers/actuator_dynamic_controller.py index 8e543e9f74..57bcddba6a 100644 --- a/smarts/core/controllers/actuator_dynamic_controller.py +++ b/smarts/core/controllers/actuator_dynamic_controller.py @@ -17,7 +17,6 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. - import numpy as np METER_PER_SECOND_TO_KM_PER_HR = 3.6 diff --git a/smarts/core/controllers/lane_following_controller.py b/smarts/core/controllers/lane_following_controller.py index 025a6f3796..f92d3c3791 100644 --- a/smarts/core/controllers/lane_following_controller.py +++ b/smarts/core/controllers/lane_following_controller.py @@ -94,7 +94,7 @@ def perform_lane_following( # This lookahead value is coupled with a few calculations below, changing it # may affect stability of the controller. wp_paths = sim.road_map.waypoint_paths( - vehicle.pose, lookahead=16, route=sensor_state.plan.route + vehicle.pose, lookahead=16, route=sensor_state.get_plan(sim.road_map).route ) assert wp_paths, "no waypoints found. not near lane?" current_lane = LaneFollowingController.find_current_lane( @@ -361,7 +361,10 @@ def perform_lane_following( ) LaneFollowingController._update_target_lane_if_reached_end_of_lane( - agent_id, vehicle, controller_state, sensor_state + agent_id, + vehicle, + controller_state, + sensor_state.get_plan(sim.road_map), ) @staticmethod @@ -438,12 +441,11 @@ def calculate_lateral_gains(sim, state, vehicle, desired_poles, target_speed): @staticmethod def _update_target_lane_if_reached_end_of_lane( - agent_id, vehicle, controller_state, sensor_state + agent_id, vehicle, controller_state, plan ): # When we reach the end of our target lane, we need to update it # to the next best lane along the path state = controller_state - plan = sensor_state.plan lane = plan.road_map.lane_by_id(state.target_lane_id) paths = lane.waypoint_paths_for_pose( vehicle.pose, lookahead=2, route=plan.route diff --git a/smarts/core/external_provider.py b/smarts/core/external_provider.py index 982ff9a5a7..184012989d 100644 --- a/smarts/core/external_provider.py +++ b/smarts/core/external_provider.py @@ -25,7 +25,7 @@ from .provider import Provider, ProviderManager, ProviderRecoveryFlags, ProviderState from .scenario import Scenario from .utils.file import replace -from .vehicle import VehicleState +from .vehicle_state import VehicleState class ExternalProvider(Provider): diff --git a/smarts/core/lidar.py b/smarts/core/lidar.py index 4dfd1bc7cb..84687a999f 100644 --- a/smarts/core/lidar.py +++ b/smarts/core/lidar.py @@ -33,12 +33,9 @@ class Lidar: """Lidar utilities.""" - def __init__( - self, origin, sensor_params: SensorParams, bullet_client: bc.BulletClient - ): + def __init__(self, origin, sensor_params: SensorParams): self._origin = origin self._sensor_params = sensor_params - self._bullet_client = bullet_client self._n_threads = psutil.cpu_count(logical=False) # As an optimization we compute a set of "base rays" once and shift translate @@ -46,6 +43,9 @@ def __init__( self._base_rays = None self._static_lidar_noise = self._compute_static_lidar_noise() + def __eq__(self, __value: object) -> bool: + return isinstance(__value, Lidar) and ((self._origin == __value._origin).all()) + @property def origin(self): """The center of the emission point of the lidar lasers.""" @@ -72,14 +72,14 @@ def _compute_static_lidar_noise(self): return np.array(static_lidar_noise, dtype=np.float64) def compute_point_cloud( - self, + self, bullet_client ) -> Tuple[List[np.ndarray], List[int], List[Tuple[np.ndarray, np.ndarray]]]: """Generate a point cloud. Returns: Point cloud of 3D points, a list of hit objects, a list of rays fired. """ rays = self._compute_rays() - point_cloud, hits = self._trace_rays(rays) + point_cloud, hits = self._trace_rays(rays, bullet_client) # point_cloud = self._apply_noise(point_cloud) assert ( len(point_cloud) == len(hits) == len(rays) == len(self._static_lidar_noise) @@ -112,14 +112,14 @@ def _compute_rays(self): ] return rays - def _trace_rays(self, rays): + def _trace_rays(self, rays, bullet_client): results = [] for batched_rays in batches( rays, int(pybullet.MAX_RAY_INTERSECTION_BATCH_SIZE - 1) ): origins, directions = zip(*batched_rays) results.extend( - self._bullet_client.rayTestBatch(origins, directions, self._n_threads) + bullet_client.rayTestBatch(origins, directions, self._n_threads) ) hit_ids, _, _, positions, _ = zip(*results) diff --git a/smarts/core/observations.py b/smarts/core/observations.py index 7549124228..ae37729bee 100644 --- a/smarts/core/observations.py +++ b/smarts/core/observations.py @@ -107,8 +107,6 @@ class RoadWaypoints(NamedTuple): class GridMapMetadata(NamedTuple): """Map grid metadata.""" - created_at: int - """Time at which the map was loaded.""" resolution: float """Map resolution in world-space-distance/cell.""" width: int @@ -210,28 +208,21 @@ class Observation(NamedTuple): """Dynamic evenly-spaced points on the road ahead of the vehicle, showing potential routes ahead.""" distance_travelled: float """Road distance driven by the vehicle.""" + road_waypoints: Optional[RoadWaypoints] + """Per-road waypoints information.""" + via_data: Vias + """Listing of nearby collectable ViaPoints and ViaPoints collected in the last step.""" # TODO: Convert to `NamedTuple` or only return point cloud. lidar_point_cloud: Optional[ Tuple[List[np.ndarray], List[bool], List[Tuple[np.ndarray, np.ndarray]]] - ] + ] = None """Lidar point cloud consisting of [points, hits, (ray_origin, ray_vector)]. Points missed (i.e., not hit) have `inf` value.""" - drivable_area_grid_map: Optional[DrivableAreaGridMap] + drivable_area_grid_map: Optional[DrivableAreaGridMap] = None """Drivable area map.""" - occupancy_grid_map: Optional[OccupancyGridMap] + occupancy_grid_map: Optional[OccupancyGridMap] = None """Occupancy map.""" - top_down_rgb: Optional[TopDownRGB] + top_down_rgb: Optional[TopDownRGB] = None """RGB camera observation.""" - road_waypoints: Optional[RoadWaypoints] - """Per-road waypoints information.""" - via_data: Vias - """Listing of nearby collectable ViaPoints and ViaPoints collected in the last step.""" signals: Optional[List[SignalObservation]] = None """List of nearby traffic signal (light) states on this timestep.""" - - -class Collision(NamedTuple): - """Represents a collision by an ego vehicle with another vehicle.""" - - # XXX: This might not work for boid agents - collidee_id: str diff --git a/smarts/core/plan.py b/smarts/core/plan.py index 00a28aa782..a0e4aaa165 100644 --- a/smarts/core/plan.py +++ b/smarts/core/plan.py @@ -24,7 +24,7 @@ import math import random from dataclasses import dataclass, field -from typing import Optional, Tuple +from typing import List, Optional, Tuple import numpy as np @@ -72,7 +72,7 @@ def is_specific(self) -> bool: """If the goal is reachable at a specific position.""" return False - def is_reached(self, vehicle) -> bool: + def is_reached(self, vehicle_state) -> bool: """If the goal has been completed.""" return False @@ -116,8 +116,8 @@ def from_road( def is_specific(self) -> bool: return True - def is_reached(self, vehicle) -> bool: - a = vehicle.position + def is_reached(self, vehicle_state) -> bool: + a = vehicle_state.pose.position b = self.position sqr_dist = (a[0] - b.x) ** 2 + (a[1] - b.y) ** 2 return sqr_dist <= self.radius**2 @@ -139,8 +139,8 @@ class TraverseGoal(Goal): def is_specific(self) -> bool: return False - def is_reached(self, vehicle) -> bool: - pose = vehicle.pose + def is_reached(self, vehicle_state) -> bool: + pose = vehicle_state.pose return self._drove_off_map(pose.point, pose.heading) def _drove_off_map(self, veh_pos: Point, veh_heading: float) -> bool: @@ -219,9 +219,9 @@ def requires_route(self) -> bool: """If the mission requires a route to be generated.""" return self.goal.is_specific() - def is_complete(self, vehicle, distance_travelled: float) -> bool: + def is_complete(self, vehicle_state, distance_travelled: float) -> bool: """If the mission has been completed successfully.""" - return self.goal.is_reached(vehicle) + return self.goal.is_reached(vehicle_state) @staticmethod def endless_mission( @@ -275,14 +275,22 @@ def __post_init__(self): # TAI: could just assert here, but may want to be more clever... self.route_length = 1 - def is_complete(self, vehicle, distance_travelled: float) -> bool: + def is_complete(self, vehicle_state, distance_travelled: float) -> bool: """If the mission has been completed.""" return ( - self.goal.is_reached(vehicle) + self.goal.is_reached(vehicle_state) and distance_travelled > self.route_length * self.num_laps ) +@dataclass +class PlanFrame: + """Describes a plan that is serializable.""" + + road_ids: List[str] + mission: Optional[Mission] + + class Plan: """Describes a navigation plan (route) to fulfill a mission.""" @@ -330,7 +338,9 @@ def create_route(self, mission: Mission, radius: Optional[float] = None): mission. Defaults to `_default_lane_width` of the underlying road_map. """ - assert not self._route, "Already called create_route()." + assert not self._route or not len( + self._route.road_ids + ), "Already called create_route()." self._mission = mission or Mission.random_endless_mission(self._road_map) if not self._mission.requires_route: @@ -377,4 +387,18 @@ def create_route(self, mission: Mission, radius: Optional[float] = None): "after a junction.".format(start_road_ids, end_lane.road.road_id) ) - return + return self._mission + + def frame(self) -> PlanFrame: + """Get the state of this plan.""" + assert self._mission + return PlanFrame( + road_ids=self._route.road_ids if self._route else [], mission=self._mission + ) + + @classmethod + def from_frame(cls, plan_frame: PlanFrame, road_map: RoadMap) -> "Plan": + """Generate the plan from a frame.""" + new_plan = cls(road_map=road_map, mission=plan_frame.mission, find_route=False) + new_plan.route = road_map.route_from_road_ids(plan_frame.road_ids) + return new_plan diff --git a/smarts/core/provider.py b/smarts/core/provider.py index d008a4f6e5..043a86e8e1 100644 --- a/smarts/core/provider.py +++ b/smarts/core/provider.py @@ -22,9 +22,8 @@ from enum import IntFlag from typing import Iterable, List, Optional, Set, Tuple -from .actor import ActorRole, ActorState +from .actor import ActorState from .controllers import ActionSpaceType -from .road_map import RoadMap from .scenario import Scenario @@ -57,13 +56,14 @@ def merge(self, other: "ProviderState"): if not our_actors.isdisjoint(other_actors): overlap = our_actors & other_actors logging.warning( - f"multiple providers control the same actors: {overlap}. " - "Later added providers will take priority. " + "multiple providers control the same actors: %s. " + "Later added providers will take priority. ", + overlap, ) logging.info( - "Conflicting actor states: \n" - f"Previous: {[(a.actor_id, a.source) for a in self.actors if a.actor_id in overlap]}\n" - f"Later: {[(a.actor_id, a.source) for a in other.actors if a.actor_id in overlap]}\n" + "Conflicting actor states: \n" "Previous: %s\n" "Later: %s\n", + [(a.actor_id, a.source) for a in self.actors if a.actor_id in overlap], + [(a.actor_id, a.source) for a in other.actors if a.actor_id in overlap], ) ## TODO: Properly harmonize these actor ids so that there is a priority and per actor source diff --git a/smarts/core/renderer.py b/smarts/core/renderer.py index 4f16a614f9..5e0c444ecc 100644 --- a/smarts/core/renderer.py +++ b/smarts/core/renderer.py @@ -147,7 +147,10 @@ def destroy(self): self.__class__.__it__ = None def __del__(self): - self.destroy() + try: + self.destroy() + except (AttributeError, TypeError): + pass def setup_sim_root(self, simid: str): """Creates the simulation root node in the scene graph.""" @@ -194,6 +197,7 @@ def __init__(self, simid: str, debug_mode: DEBUG_MODE = DEBUG_MODE.ERROR): self._road_map_np = None self._dashed_lines_np = None self._vehicle_nodes = {} + self._camera_nodes = {} _ShowBaseInstance.set_rendering_verbosity(debug_mode=debug_mode) # Note: Each instance of the SMARTS simulation will have its own Renderer, # but all Renderer objects share the same ShowBaseInstance. @@ -266,8 +270,10 @@ def setup(self, scenario: Scenario): # Load map if self._road_map_np: self._log.debug( - "road_map={} already exists. Removing and adding a new " - "one from glb_path={}".format(self._road_map_np, map_path) + "road_map=%s already exists. Removing and adding a new " + "one from glb_path=%s", + self._road_map_np, + map_path, ) map_np = self._showbase_instance.loader.loadModel(map_path, noCache=True) np = self._root_np.attachNewNode("road_map") @@ -323,10 +329,26 @@ def render(self): assert self._is_setup self._showbase_instance.render_node(self._root_np) + def reset(self): + """Reset the render back to initialized state.""" + self._vehicles_np.removeNode() + self._vehicles_np = self._root_np.attachNewNode("vehicles") + self._vehicle_nodes = {} + def step(self): """provided for non-SMARTS uses; normally not used by SMARTS.""" self._showbase_instance.taskMgr.step() + def sync(self, sim_frame): + """Update the current state of the vehicles within the renderer.""" + for vehicle_id, vehicle_state in sim_frame.vehicle_states.items(): + self.update_vehicle_node(vehicle_id, vehicle_state.pose) + + missing_vehicle_ids = set(self._vehicle_nodes) - set(sim_frame.vehicle_ids) + + for vid in missing_vehicle_ids: + self.remove_vehicle_node(vid) + def teardown(self): """Clean up internal resources.""" if self._root_np is not None: @@ -361,6 +383,8 @@ def create_vehicle_node( self, glb_model: str, vid: str, color: Union[Colors, SceneColors], pose: Pose ): """Create a vehicle node.""" + if vid in self._vehicle_nodes: + return False with pkg_resources.path(models, glb_model) as path: node_path = self._showbase_instance.loader.loadModel(str(path.absolute())) node_path.setName("vehicle-%s" % vid) @@ -376,6 +400,7 @@ def create_vehicle_node( node_path.setPosHpr(*pos, heading, 0, 0) node_path.hide(RenderMasks.DRIVABLE_AREA_HIDE) self._vehicle_nodes[vid] = node_path + return True def begin_rendering_vehicle(self, vid: str, is_agent: bool): """Add the vehicle node to the scene graph""" @@ -383,8 +408,7 @@ def begin_rendering_vehicle(self, vid: str, is_agent: bool): if not vehicle_path: self._log.warning("Renderer ignoring invalid vehicle id: %s", vid) return - # TAI: consider reparenting hijacked vehicles too? - vehicle_path.reparentTo(self._vehicles_np if is_agent else self._root_np) + vehicle_path.reparentTo(self._vehicles_np) def update_vehicle_node(self, vid: str, pose: Pose): """Move the specified vehicle node.""" @@ -399,9 +423,18 @@ def remove_vehicle_node(self, vid: str): """Remove a vehicle node""" vehicle_path = self._vehicle_nodes.get(vid, None) if not vehicle_path: - self._log.warning(f"Renderer ignoring invalid vehicle id: {vid}") + self._log.warning("Renderer ignoring invalid vehicle id: %s", vid) return vehicle_path.removeNode() + del self._vehicle_nodes[vid] + + def camera_for_id(self, camera_id) -> "Renderer.OffscreenCamera": + """Get a camera by its id.""" + camera = self._camera_nodes.get(camera_id) + assert ( + camera is not None + ), f"Camera {camera_id} does not exist, have you created this camera?" + return camera class OffscreenCamera(NamedTuple): """A camera used for rendering images to a graphics buffer.""" @@ -511,4 +544,7 @@ def build_offscreen_camera( # mask is set to make undesirable objects invisible to this camera camera_np.node().setCameraMask(camera_np.node().getCameraMask() & mask) - return Renderer.OffscreenCamera(camera_np, buffer, tex, self) + camera = Renderer.OffscreenCamera(camera_np, buffer, tex, self) + self._camera_nodes[name] = camera + + return name diff --git a/smarts/core/road_map.py b/smarts/core/road_map.py index 6f2d168a26..5fa3be671a 100644 --- a/smarts/core/road_map.py +++ b/smarts/core/road_map.py @@ -76,6 +76,11 @@ def dynamic_features(self) -> List[RoadMap.Feature]: """All dynamic features associated with this road map.""" return [] + @property + def map_spec(self): + """The map spec that could be used to reconstruct the map.""" + return getattr(self, "_map_spec", None) + def is_same_map(self, map_spec) -> bool: """Check if the MapSpec Object source points to the same RoadMap instance as the current""" raise NotImplementedError diff --git a/smarts/core/scenario.py b/smarts/core/scenario.py index cf6591fc45..af0791bcaa 100644 --- a/smarts/core/scenario.py +++ b/smarts/core/scenario.py @@ -105,7 +105,7 @@ def __init__( scenario_root: str, traffic_specs: Sequence[str] = [], missions: Optional[Dict[str, Mission]] = None, - social_agents: Optional[Dict[str, SocialAgent]] = None, + social_agents: Optional[Dict[str, Tuple[Any, SocialAgent]]] = None, log_dir: Optional[str] = None, surface_patches: Optional[Sequence[Dict[str, Any]]] = None, traffic_history: Optional[str] = None, @@ -989,6 +989,11 @@ def road_map(self) -> RoadMap: """The road map of the scenario.""" return self._road_map + @property + def map_spec(self) -> Optional[MapSpec]: + """The map spec for the road map used in this scenario.""" + return self.road_map.map_spec + @property def supports_sumo_traffic(self) -> bool: """Returns True if this scenario uses a Sumo road network.""" diff --git a/smarts/core/sensor.py b/smarts/core/sensor.py new file mode 100644 index 0000000000..6eadc2efc6 --- /dev/null +++ b/smarts/core/sensor.py @@ -0,0 +1,796 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging +import sys +from collections import deque +from dataclasses import dataclass +from functools import lru_cache +from typing import List, Optional, Tuple + +import numpy as np + +from smarts.core.coordinates import Pose, RefLinePoint +from smarts.core.lidar import Lidar +from smarts.core.lidar_sensor_params import SensorParams +from smarts.core.masks import RenderMasks +from smarts.core.observations import ( + DrivableAreaGridMap, + EgoVehicleObservation, + GridMapMetadata, + Observation, + OccupancyGridMap, + RoadWaypoints, + SignalObservation, + TopDownRGB, + VehicleObservation, + ViaPoint, + Vias, +) +from smarts.core.plan import Plan +from smarts.core.road_map import RoadMap, Waypoint +from smarts.core.signals import SignalState +from smarts.core.utils.math import squared_dist +from smarts.core.vehicle_state import VehicleState, neighborhood_vehicles_around_vehicle + + +class Sensor: + """The sensor base class.""" + + def step(self, sim_frame, **kwargs): + """Update sensor state.""" + pass + + def teardown(self, **kwargs): + """Clean up internal resources""" + raise NotImplementedError + + @property + def mutable(self) -> bool: + """If this sensor mutates on call.""" + return True + + @property + def serializable(self) -> bool: + """If this sensor can be serialized.""" + return True + + +class CameraSensor(Sensor): + """The base for a sensor that renders images.""" + + def __init__( + self, + vehicle_state, + renderer, # type Renderer or None + name: str, + mask: int, + width: int, + height: int, + resolution: float, + ): + assert renderer + self._log = logging.getLogger(self.__class__.__name__) + self._camera_name = renderer.build_offscreen_camera( + f"{name}_{vehicle_state.actor_id}", + mask, + width, + height, + resolution, + ) + self._target_actor = vehicle_state.actor_id + self._follow_actor( + vehicle_state, renderer + ) # ensure we have a correct initial camera position + + def __eq__(self, __value: object) -> bool: + return ( + isinstance(__value, CameraSensor) + and self._target_actor == self._target_actor + ) + + def teardown(self, **kwargs): + renderer = kwargs.get("renderer") + if not renderer: + return + camera = renderer.camera_for_id(self._camera_name) + camera.teardown() + + def step(self, sim_frame, **kwargs): + if not self._target_actor in sim_frame.actor_states_by_id: + return + self._follow_actor( + sim_frame.actor_states_by_id[self._target_actor], kwargs.get("renderer") + ) + + def _follow_actor(self, vehicle_state, renderer): + if not renderer: + return + camera = renderer.camera_for_id(self._camera_name) + camera.update(vehicle_state.pose, vehicle_state.dimensions.height + 10) + + @property + def serializable(self) -> bool: + return False + + +class DrivableAreaGridMapSensor(CameraSensor): + """A sensor that renders drivable area from around its target actor.""" + + def __init__( + self, + vehicle_state, + width: int, + height: int, + resolution: float, + renderer, # type Renderer or None + ): + super().__init__( + vehicle_state, + renderer, + "drivable_area_grid_map", + RenderMasks.DRIVABLE_AREA_HIDE, + width, + height, + resolution, + ) + self._resolution = resolution + + def __call__(self, renderer) -> DrivableAreaGridMap: + camera = renderer.camera_for_id(self._camera_name) + assert camera is not None, "Drivable area grid map has not been initialized" + + ram_image = camera.wait_for_ram_image(img_format="A") + mem_view = memoryview(ram_image) + image = np.frombuffer(mem_view, np.uint8) + image.shape = (camera.tex.getYSize(), camera.tex.getXSize(), 1) + image = np.flipud(image) + + metadata = GridMapMetadata( + resolution=self._resolution, + height=image.shape[0], + width=image.shape[1], + camera_position=camera.camera_np.getPos(), + camera_heading_in_degrees=camera.camera_np.getH(), + ) + return DrivableAreaGridMap(data=image, metadata=metadata) + + +class OGMSensor(CameraSensor): + """A sensor that renders occupancy information from around its target actor.""" + + def __init__( + self, + vehicle_state, + width: int, + height: int, + resolution: float, + renderer, # type Renderer or None + ): + super().__init__( + vehicle_state, + renderer, + "ogm", + RenderMasks.OCCUPANCY_HIDE, + width, + height, + resolution, + ) + self._resolution = resolution + + def __call__(self, renderer) -> OccupancyGridMap: + camera = renderer.camera_for_id(self._camera_name) + assert camera is not None, "OGM has not been initialized" + + ram_image = camera.wait_for_ram_image(img_format="A") + mem_view = memoryview(ram_image) + grid = np.frombuffer(mem_view, np.uint8) + grid.shape = (camera.tex.getYSize(), camera.tex.getXSize(), 1) + grid = np.flipud(grid) + + metadata = GridMapMetadata( + resolution=self._resolution, + height=grid.shape[0], + width=grid.shape[1], + camera_position=camera.camera_np.getPos(), + camera_heading_in_degrees=camera.camera_np.getH(), + ) + return OccupancyGridMap(data=grid, metadata=metadata) + + +class RGBSensor(CameraSensor): + """A sensor that renders color values from around its target actor.""" + + def __init__( + self, + vehicle_state, + width: int, + height: int, + resolution: float, + renderer, # type Renderer or None + ): + super().__init__( + vehicle_state, + renderer, + "top_down_rgb", + RenderMasks.RGB_HIDE, + width, + height, + resolution, + ) + self._resolution = resolution + + def __call__(self, renderer) -> TopDownRGB: + camera = renderer.camera_for_id(self._camera_name) + assert camera is not None, "RGB has not been initialized" + + ram_image = camera.wait_for_ram_image(img_format="RGB") + mem_view = memoryview(ram_image) + image = np.frombuffer(mem_view, np.uint8) + image.shape = (camera.tex.getYSize(), camera.tex.getXSize(), 3) + image = np.flipud(image) + + metadata = GridMapMetadata( + resolution=self._resolution, + height=image.shape[0], + width=image.shape[1], + camera_position=camera.camera_np.getPos(), + camera_heading_in_degrees=camera.camera_np.getH(), + ) + return TopDownRGB(data=image, metadata=metadata) + + +class LidarSensor(Sensor): + """A lidar sensor.""" + + def __init__( + self, + vehicle_state: VehicleState, + sensor_params: Optional[SensorParams] = None, + lidar_offset=(0, 0, 1), + ): + self._lidar_offset = np.array(lidar_offset) + + self._lidar = Lidar( + vehicle_state.pose.position + self._lidar_offset, + sensor_params, + ) + + def follow_vehicle(self, vehicle_state: VehicleState): + """Update the sensor to target the given vehicle.""" + self._lidar.origin = vehicle_state.pose.position + self._lidar_offset + + def __eq__(self, __value: object) -> bool: + return isinstance(__value, LidarSensor) and ( + (self._lidar_offset == __value._lidar_offset).all() + and (self._lidar.origin == __value._lidar.origin).all() + ) + + def __call__(self, bullet_client): + return self._lidar.compute_point_cloud(bullet_client) + + def teardown(self, **kwargs): + pass + + @property + def serializable(self) -> bool: + return False + + +@dataclass(frozen=True) +class _DrivenPathSensorEntry: + timestamp: float + position: Tuple[float, float] + + +class DrivenPathSensor(Sensor): + """Tracks the driven path as a series of positions (regardless if the vehicle is + following the route or not). For performance reasons it only keeps the last + N=max_path_length path segments. + """ + + def __init__(self, max_path_length: int = 500): + self._driven_path = deque(maxlen=max_path_length) + + def track_latest_driven_path(self, elapsed_sim_time, vehicle_state): + """Records the current location of the tracked vehicle.""" + position = vehicle_state.pose.position[:2] + self._driven_path.append( + _DrivenPathSensorEntry(timestamp=elapsed_sim_time, position=tuple(position)) + ) + + def __call__(self, count=sys.maxsize): + return [x.position for x in self._driven_path][-count:] + + def teardown(self, **kwargs): + pass + + def __eq__(self, __value: object) -> bool: + return ( + isinstance(__value, DrivenPathSensor) + and self._driven_path == __value._driven_path + ) + + def distance_travelled( + self, + elapsed_sim_time, + last_n_seconds: Optional[float] = None, + last_n_steps: Optional[int] = None, + ): + """Find the amount of distance travelled over the last # of seconds XOR steps""" + if last_n_seconds is None and last_n_steps is None: + raise ValueError("Either last N seconds or last N steps must be provided") + + if last_n_steps is not None: + n = last_n_steps + 1 # to factor in the current step we're on + filtered_pos = [x.position for x in self._driven_path][-n:] + else: # last_n_seconds + threshold = elapsed_sim_time - last_n_seconds + filtered_pos = [ + x.position for x in self._driven_path if x.timestamp >= threshold + ] + + xs = np.array([p[0] for p in filtered_pos]) + ys = np.array([p[1] for p in filtered_pos]) + dist_array = (xs[:-1] - xs[1:]) ** 2 + (ys[:-1] - ys[1:]) ** 2 + return np.sum(np.sqrt(dist_array)) + + +class TripMeterSensor(Sensor): + """Tracks distance travelled along the route (in meters). Meters driven while + off-route are not counted as part of the total. + """ + + def __init__(self): + self._wps_for_distance: List[Waypoint] = [] + self._dist_travelled = 0.0 + self._last_dist_travelled = 0.0 + self._last_actor_position = None + + def __eq__(self, __value: object) -> bool: + return isinstance(__value, TripMeterSensor) and ( + self._wps_for_distance == __value._wps_for_distance + and self._dist_travelled == __value._dist_travelled + and self._last_dist_travelled == __value._last_dist_travelled + ) + + def update_distance_wps_record( + self, waypoint_paths, vehicle, plan: Plan, road_map: RoadMap + ): + """Append a waypoint to the history if it is not already counted.""" + # Distance calculation. Intention is the shortest trip travelled at the lane + # level the agent has travelled. This is to prevent lateral movement from + # increasing the total distance travelled. + self._last_dist_travelled = self._dist_travelled + + new_wp = waypoint_paths[0][0] + wp_road = road_map.lane_by_id(new_wp.lane_id).road.road_id + + should_count_wp = ( + # if we do not have a fixed route, we count all waypoints we accumulate + not plan.mission.requires_route + # if we have a route to follow, only count wps on route + or wp_road in [road.road_id for road in plan.route.roads] + ) + + if not self._wps_for_distance: + self._last_actor_position = vehicle.pose.position + if should_count_wp: + self._wps_for_distance.append(new_wp) + return # sensor does not have enough history + most_recent_wp = self._wps_for_distance[-1] + + # TODO: Instead of storing a waypoint every 0.5m just find the next one immediately + threshold_for_counting_wp = 0.5 # meters from last tracked waypoint + if ( + np.linalg.norm(new_wp.pos - most_recent_wp.pos) > threshold_for_counting_wp + and should_count_wp + ): + self._wps_for_distance.append(new_wp) + additional_distance = TripMeterSensor._compute_additional_dist_travelled( + most_recent_wp, + new_wp, + vehicle.pose.position, + self._last_actor_position, + ) + self._dist_travelled += additional_distance + self._last_actor_position = vehicle.pose.position + + @staticmethod + def _compute_additional_dist_travelled( + recent_wp: Waypoint, + new_waypoint: Waypoint, + vehicle_position: np.ndarray, + last_vehicle_pos: np.ndarray, + ): + # old waypoint minus current ahead waypoint + wp_disp_vec = new_waypoint.pos - recent_wp.pos + # make unit vector + wp_unit_vec = wp_disp_vec / (np.linalg.norm(wp_disp_vec) or 1) + # vehicle position minus last vehicle position + position_disp_vec = vehicle_position[:2] - last_vehicle_pos[:2] + # distance of vehicle between last and current wp + distance = np.dot(position_disp_vec, wp_unit_vec) + return distance + + def __call__(self, increment=False): + if increment: + return self._dist_travelled - self._last_dist_travelled + + return self._dist_travelled + + def teardown(self, **kwargs): + pass + + +class NeighborhoodVehiclesSensor(Sensor): + """Detects other vehicles around the sensor equipped vehicle.""" + + def __init__(self, radius=None): + self._radius = radius + + @property + def radius(self): + """Radius to check for nearby vehicles.""" + return self._radius + + def __call__(self, vehicle_state: VehicleState, vehicle_states): + return neighborhood_vehicles_around_vehicle( + vehicle_state, vehicle_states, radius=self._radius + ) + + def __eq__(self, __value: object) -> bool: + return ( + isinstance(__value, NeighborhoodVehiclesSensor) + and self._radius == __value._radius + ) + + def teardown(self, **kwargs): + pass + + @property + def mutable(self) -> bool: + return False + + +class WaypointsSensor(Sensor): + """Detects waypoints leading forward along the vehicle plan.""" + + def __init__(self, lookahead=32): + self._lookahead = lookahead + + def __call__(self, vehicle_state: VehicleState, plan: Plan, road_map): + return road_map.waypoint_paths( + pose=vehicle_state.pose, + lookahead=self._lookahead, + route=plan.route, + ) + + def __eq__(self, __value: object) -> bool: + return ( + isinstance(__value, WaypointsSensor) + and self._lookahead == __value._lookahead + ) + + def teardown(self, **kwargs): + pass + + @property + def mutable(self) -> bool: + return False + + +class RoadWaypointsSensor(Sensor): + """Detects waypoints from all paths nearby the vehicle.""" + + def __init__(self, horizon=32): + self._horizon = horizon + + def __call__(self, vehicle_state: VehicleState, plan, road_map) -> RoadWaypoints: + veh_pt = vehicle_state.pose.point + lane = road_map.nearest_lane(veh_pt) + if not lane: + return RoadWaypoints(lanes={}) + road = lane.road + lane_paths = {} + for croad in ( + [road] + road.parallel_roads + road.oncoming_roads_at_point(veh_pt) + ): + for lane in croad.lanes: + lane_paths[lane.lane_id] = self._paths_for_lane( + lane, vehicle_state, plan + ) + + return RoadWaypoints(lanes=lane_paths) + + def _paths_for_lane( + self, lane, vehicle_state: VehicleState, plan, overflow_offset=None + ): + """Gets waypoint paths along the given lane.""" + # XXX: the following assumes waypoint spacing is 1m + if overflow_offset is None: + offset = lane.offset_along_lane(vehicle_state.pose.point) + start_offset = offset - self._horizon + else: + start_offset = lane.length + overflow_offset + + incoming_lanes = lane.incoming_lanes + if start_offset < 0 and len(incoming_lanes) > 0: + paths = [] + for lane in incoming_lanes: + paths += self._paths_for_lane(lane, vehicle_state, plan, start_offset) + return paths + else: + start_offset = max(0, start_offset) + wp_start = lane.from_lane_coord(RefLinePoint(start_offset)) + adj_pose = Pose.from_center(wp_start, vehicle_state.pose.heading) + wps_to_lookahead = self._horizon * 2 + paths = lane.waypoint_paths_for_pose( + pose=adj_pose, + lookahead=wps_to_lookahead, + route=plan.route, + ) + return paths + + def __eq__(self, __value: object) -> bool: + return isinstance(__value, RoadWaypoints) and self._horizon == __value._horizon + + def teardown(self, **kwargs): + pass + + @property + def mutable(self) -> bool: + return False + + +class AccelerometerSensor(Sensor): + """Tracks motion changes within the vehicle equipped with this sensor.""" + + def __init__(self): + self.linear_velocities = deque(maxlen=3) + self.angular_velocities = deque(maxlen=3) + + def __call__(self, linear_velocity, angular_velocity, dt: float): + if linear_velocity is not None: + self.linear_velocities.append(linear_velocity) + if angular_velocity is not None: + self.angular_velocities.append(angular_velocity) + + linear_acc = np.array((0.0, 0.0, 0.0)) + angular_acc = np.array((0.0, 0.0, 0.0)) + linear_jerk = np.array((0.0, 0.0, 0.0)) + angular_jerk = np.array((0.0, 0.0, 0.0)) + + if not dt: + return (linear_acc, angular_acc, linear_jerk, angular_jerk) + + if len(self.linear_velocities) >= 2: + linear_acc = (self.linear_velocities[-1] - self.linear_velocities[-2]) / dt + if len(self.linear_velocities) >= 3: + last_linear_acc = ( + self.linear_velocities[-2] - self.linear_velocities[-3] + ) / dt + linear_jerk = linear_acc - last_linear_acc + if len(self.angular_velocities) >= 2: + angular_acc = ( + self.angular_velocities[-1] - self.angular_velocities[-2] + ) / dt + if len(self.angular_velocities) >= 3: + last_angular_acc = ( + self.angular_velocities[-2] - self.angular_velocities[-3] + ) / dt + angular_jerk = angular_acc - last_angular_acc + + return (linear_acc, angular_acc, linear_jerk, angular_jerk) + + def __eq__(self, __value: object) -> bool: + return isinstance(__value, AccelerometerSensor) and ( + [ + (a == b).all() + for a, b in zip(self.linear_velocities, __value.linear_velocities) + ] + and [ + (a == b).all() + for a, b in zip(self.angular_velocities, __value.angular_velocities) + ] + ) + + def teardown(self, **kwargs): + pass + + +class LanePositionSensor(Sensor): + """Tracks lane-relative RefLine (Frenet) coordinates.""" + + def __init__(self): + pass + + def __call__(self, lane: RoadMap.Lane, vehicle_state): + return lane.to_lane_coord(vehicle_state.pose.point) + + def __eq__(self, __value: object) -> bool: + return True + + def teardown(self, **kwargs): + pass + + @property + def mutable(self) -> bool: + return False + + +class ViaSensor(Sensor): + """Tracks collection of ViaPoint collectables""" + + def __init__(self, lane_acquisition_range, speed_accuracy): + self._consumed_via_points = set() + self._acquisition_range = lane_acquisition_range + self._speed_accuracy = speed_accuracy + + def __eq__(self, __value: object) -> bool: + return isinstance(__value, ViaSensor) and ( + self._consumed_via_points == __value._consumed_via_points + and self._acquisition_range == __value._acquisition_range + and self._speed_accuracy == __value._speed_accuracy + ) + + def __call__(self, vehicle_state: VehicleState, plan, road_map): + near_points: List[ViaPoint] = list() + hit_points: List[ViaPoint] = list() + vehicle_position = vehicle_state.pose.position[:2] + + @lru_cache() + def closest_point_on_lane(position, lane_id, road_map): + lane = road_map.lane_by_id(lane_id) + return lane.center_at_point(position) + + for via in plan.mission.via: + closest_position_on_lane = closest_point_on_lane( + tuple(vehicle_position), via.lane_id, road_map + ) + closest_position_on_lane = closest_position_on_lane[:2] + + dist_from_lane_sq = squared_dist(vehicle_position, closest_position_on_lane) + if dist_from_lane_sq > self._acquisition_range**2: + continue + + point = ViaPoint( + tuple(via.position), + lane_index=via.lane_index, + road_id=via.road_id, + required_speed=via.required_speed, + ) + + near_points.append(point) + dist_from_point_sq = squared_dist(vehicle_position, via.position) + if ( + dist_from_point_sq <= via.hit_distance**2 + and via not in self._consumed_via_points + and np.isclose( + vehicle_state.speed, via.required_speed, atol=self._speed_accuracy + ) + ): + self._consumed_via_points.add(via) + hit_points.append(point) + + return ( + sorted( + near_points, + key=lambda point: squared_dist(point.position, vehicle_position), + ), + hit_points, + ) + + def teardown(self, **kwargs): + pass + + +class SignalsSensor(Sensor): + """Reports state of traffic signals (lights) in the lanes ahead of vehicle.""" + + def __init__(self, lookahead: float): + self._lookahead = lookahead + + def __eq__(self, __value: object) -> bool: + return ( + isinstance(__value, SignalsSensor) and self._lookahead == __value._lookahead + ) + + @staticmethod + def _is_signal_type(feature: RoadMap.Feature) -> bool: + # XXX: eventually if we add other types of dynamic features, we'll need to update this. + return ( + feature.type == RoadMap.FeatureType.FIXED_LOC_SIGNAL or feature.is_dynamic + ) + + def __call__( + self, + lane: Optional[RoadMap.Lane], + lane_pos: RefLinePoint, + state: VehicleState, + plan: Plan, + provider_state, # ProviderState + ) -> List[SignalObservation]: + result = [] + if not lane: + return result + upcoming_signals = [] + for feat in lane.features: + if not self._is_signal_type(feat): + continue + for pt in feat.geometry: + # TAI: we might want to use the position of our back bumper + # instead of centroid to allow agents to have some (even more) + # imprecision in their handling of stopping at signals. + if lane.offset_along_lane(pt) >= lane_pos.s: + upcoming_signals.append(feat) + break + lookahead = self._lookahead - lane.length + lane_pos.s + self._find_signals_ahead(lane, lookahead, plan.route, upcoming_signals) + + for signal in upcoming_signals: + for actor_state in provider_state.actors: + if actor_state.actor_id == signal.feature_id: + signal_state = actor_state + break + else: + logger = logging.getLogger(self.__class__.__name__) + logger.warning( + "could not find signal state corresponding with feature_id=%s}", + signal.feature_id, + ) + continue + assert isinstance(signal_state, SignalState) + controlled_lanes = None + if signal_state.controlled_lanes: + controlled_lanes = signal_state.controlled_lanes + result.append( + SignalObservation( + state=signal_state.state, + stop_point=signal_state.stopping_pos, + controlled_lanes=controlled_lanes, + last_changed=signal_state.last_changed, + ) + ) + + return result + + def _find_signals_ahead( + self, + lane: RoadMap.Lane, + lookahead: float, + route: Optional[RoadMap.Route], + upcoming_signals: List[RoadMap.Feature], + ): + if lookahead <= 0: + return + for ogl in lane.outgoing_lanes: + if route and route.road_length > 0 and ogl.road not in route.roads: + continue + upcoming_signals += [ + feat for feat in ogl.features if self._is_signal_type(feat) + ] + self._find_signals_ahead( + ogl, lookahead - lane.length, route, upcoming_signals + ) + + def teardown(self, **kwargs): + pass diff --git a/smarts/core/sensor_manager.py b/smarts/core/sensor_manager.py new file mode 100644 index 0000000000..267528f2d9 --- /dev/null +++ b/smarts/core/sensor_manager.py @@ -0,0 +1,286 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging +from collections import Counter +from typing import Dict, FrozenSet, List, Optional, Set, Tuple + +from smarts.core import config +from smarts.core.sensors import Observation, Sensor, Sensors, SensorState +from smarts.core.sensors.local_sensor_resolver import LocalSensorResolver +from smarts.core.sensors.parallel_sensor_resolver import ParallelSensorResolver +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants + +logger = logging.getLogger(__name__) + + +class SensorManager: + """A sensor management system that associates actors with sensors.""" + + def __init__(self): + self._sensors: Dict[str, Sensor] = {} + + # {actor_id: } + self._sensor_states: Dict[str, SensorState] = {} + + # {actor_id: {sensor_id, ...}} + self._sensors_by_actor_id: Dict[str, Set[str]] = {} + self._actors_by_sensor_id: Dict[str, Set[str]] = {} + self._sensor_references = Counter() + # {sensor_id, ...} + self._discarded_sensors: Set[str] = set() + observation_workers = config()( + "core", "observation_workers", default=0, cast=int + ) + parallel_resolver = ParallelSensorResolver + if ( + backing := config()("core", "sensor_parallelization", default="mp") + ) == "ray": + try: + import ray + + from smarts.ray.sensors.ray_sensor_resolver import RaySensorResolver + + parallel_resolver = RaySensorResolver + except ImportError: + pass + elif backing == "mp": + pass + else: + raise LookupError( + f"SMARTS_CORE_SENSOR_PARALLELIZATION={backing} is not a valid option." + ) + self._sensor_resolver = ( + parallel_resolver() if observation_workers > 0 else LocalSensorResolver() + ) + + def step(self, sim_frame: SimulationFrame, renderer): + """Update sensor values based on the new simulation state.""" + self._sensor_resolver.step(sim_frame, self._sensor_states.values()) + + for sensor in self._sensors.values(): + sensor.step(sim_frame=sim_frame, renderer=renderer) + + def observe( + self, + sim_frame, + sim_local_constants, + agent_ids, + renderer_ref, + physics_ref, + ): + """Runs observations and updates the sensor states. + Args: + sim_frame (SimulationFrame): + The current state from the simulation. + sim_local_constants (SimulationLocalConstants): + The values that should stay the same for a simulation over a reset. + agent_ids ({str, ...}): + The agent ids to process. + renderer_ref (Optional[Renderer]): + The renderer (if any) that should be used. + physics_ref (bc.BulletClient): + The physics client. + """ + observations, dones, updated_sensors = self._sensor_resolver.observe( + sim_frame, + sim_local_constants, + agent_ids, + renderer_ref, + physics_ref, + ) + for actor_id, sensors in updated_sensors.items(): + for sensor_name, sensor in sensors.items(): + self._sensors[ + SensorManager._actor_and_sensor_name_to_sensor_id( + sensor_name, actor_id + ) + ] = sensor + + return observations, dones + + def observe_batch( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_id, + sensor_states, + vehicles, + renderer, + bullet_client, + ) -> Tuple[Dict[str, Observation], Dict[str, bool]]: + """Operates all sensors on a batch of vehicles for a single agent.""" + # TODO: Replace this with a more efficient implementation that _actually_ + # does batching + assert sensor_states.keys() == vehicles.keys() + + observations, dones = {}, {} + for vehicle_id, vehicle in vehicles.items(): + sensor_state = sensor_states[vehicle_id] + ( + observations[vehicle_id], + dones[vehicle_id], + updated_sensors, + ) = Sensors.observe_vehicle( + sim_frame, + sim_local_constants, + agent_id, + sensor_state, + vehicle, + renderer, + bullet_client, + ) + for sensor_name, sensor in updated_sensors.items(): + self._sensors[ + SensorManager._actor_and_sensor_name_to_sensor_id( + sensor_name, vehicle_id + ) + ] = sensor + + return observations, dones + + def teardown(self, renderer): + """Tear down the current sensors and clean up any internal resources.""" + for sensor in self._sensors.values(): + sensor.teardown(renderer=renderer) + self._sensors = {} + self._sensor_states = {} + self._sensors_by_actor_id = {} + self._sensor_references.clear() + self._discarded_sensors.clear() + + def add_sensor_state(self, actor_id: str, sensor_state: SensorState): + """Add a sensor state associated with a given actor.""" + self._sensor_states[actor_id] = sensor_state + + def remove_sensors_by_actor_id(self, actor_id: str) -> FrozenSet[str]: + """Remove association of an actor to sensors. If the sensor is no longer associated the + sensor is scheduled to be removed.""" + sensor_states = self._sensor_states.get(actor_id) + if not sensor_states: + logger.warning( + "Attempted to remove sensors from actor with no sensors: `%s`", actor_id + ) + return frozenset() + del self._sensor_states[actor_id] + sensors_by_actor = self._sensors_by_actor_id[actor_id] + for sensor_id in sensors_by_actor: + self._sensor_references.subtract([sensor_id]) + count = self._sensor_references[sensor_id] + self._actors_by_sensor_id[sensor_id].remove(actor_id) + if count < 1: + self._discarded_sensors.add(sensor_id) + del self._sensors_by_actor_id[actor_id] + return frozenset(self._discarded_sensors) + + def remove_sensor(self, sensor_id: str) -> Optional[Sensor]: + """Remove a sensor by its id. Removes any associations it has with actors.""" + sensor = self._sensors.get(sensor_id) + if not sensor: + return None + del self._sensors[sensor_id] + del self._sensor_references[sensor_id] + + ## clean up any remaining references by actors + if sensor_id in self._actors_by_sensor_id: + for actor_id in self._actors_by_sensor_id[sensor_id]: + self._sensors_by_actor_id[actor_id].remove(sensor_id) + del self._actors_by_sensor_id[sensor_id] + return sensor + + def sensor_state_exists(self, actor_id: str) -> bool: + """Determines if a actor has a sensor state associated with it.""" + return actor_id in self._sensor_states + + def sensor_states_items(self): + """Gets all actor to sensor state associations.""" + return self._sensor_states.items() + + def sensors_for_actor_id(self, actor_id: str) -> List[Sensor]: + """Gets all sensors associated with the given actor.""" + return [ + self._sensors[s_id] + for s_id in self._sensors_by_actor_id.get(actor_id, set()) + ] + + def sensors_for_actor_ids( + self, actor_ids: Set[str] + ) -> Dict[str, Dict[str, Sensor]]: + """Gets all sensors for the given actors.""" + return { + actor_id: { + SensorManager._actor_sid_to_sname(s_id): self._sensors[s_id] + for s_id in self._sensors_by_actor_id.get(actor_id, set()) + } + for actor_id in actor_ids + } + + def sensor_state_for_actor_id(self, actor_id: str): + """Gets the sensor state for the given actor.""" + return self._sensor_states.get(actor_id) + + @staticmethod + def _actor_sid_to_sname(sensor_id: str): + return sensor_id.partition("-")[0] + + @staticmethod + def _actor_and_sensor_name_to_sensor_id(sensor_name, actor_id): + return f"{sensor_name}-{actor_id}" + + def add_sensor_for_actor(self, actor_id: str, name: str, sensor: Sensor) -> str: + """Adds a sensor association for a specific actor.""" + # TAI: Allow multiple sensors of the same type on the same actor + s_id = SensorManager._actor_and_sensor_name_to_sensor_id(name, actor_id) + actor_sensors = self._sensors_by_actor_id.setdefault(actor_id, set()) + if s_id in actor_sensors: + logger.warning( + "Duplicate sensor attempted to add to actor `%s`: `%s`", actor_id, s_id + ) + return s_id + actor_sensors.add(s_id) + actors = self._actors_by_sensor_id.setdefault(s_id, set()) + actors.add(actor_id) + return self.add_sensor(s_id, sensor) + + def add_sensor(self, sensor_id, sensor: Sensor) -> str: + """Adds a sensor to the sensor manager.""" + assert sensor_id not in self._sensors + self._sensors[sensor_id] = sensor + self._sensor_references.update([sensor_id]) + return sensor_id + + def clean_up_sensors_for_actors(self, current_actor_ids: Set[str], renderer): + """Cleans up sensors that are attached to non-existing actors.""" + # This is not good enough by itself since actors can keep alive sensors that are not in use by an agent + old_actor_ids = set(self._sensor_states) + missing_actors = old_actor_ids - current_actor_ids + + for aid in missing_actors: + self.remove_sensors_by_actor_id(aid) + + for sensor_id in self._discarded_sensors: + if self._sensor_references.get(sensor_id, 0) < 1: + sensor = self.remove_sensor(sensor_id) + if sensor is not None: + sensor.teardown(renderer=renderer) + + self._discarded_sensors.clear() diff --git a/smarts/core/sensors.py b/smarts/core/sensors.py deleted file mode 100644 index 4e221e21fd..0000000000 --- a/smarts/core/sensors.py +++ /dev/null @@ -1,1228 +0,0 @@ -# Copyright (C) 2020. Huawei Technologies Co., Ltd. All rights reserved. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -import logging -import re -import sys -import time -import weakref -from collections import deque, namedtuple -from dataclasses import dataclass -from functools import lru_cache -from typing import Dict, Iterable, List, Optional, Set, Tuple - -import numpy as np - -from smarts.core.agent_interface import ActorsAliveDoneCriteria, AgentsAliveDoneCriteria -from smarts.core.plan import Plan -from smarts.core.road_map import RoadMap, Waypoint -from smarts.core.signals import SignalState -from smarts.core.utils.math import squared_dist - -from .coordinates import Heading, Point, Pose, RefLinePoint -from .events import Events -from .lidar import Lidar -from .lidar_sensor_params import SensorParams -from .masks import RenderMasks -from .observations import ( - DrivableAreaGridMap, - EgoVehicleObservation, - GridMapMetadata, - Observation, - OccupancyGridMap, - RoadWaypoints, - SignalObservation, - TopDownRGB, - VehicleObservation, - ViaPoint, - Vias, -) -from .plan import Via - -logger = logging.getLogger(__name__) - -LANE_ID_CONSTANT = "off_lane" -ROAD_ID_CONSTANT = "off_road" -LANE_INDEX_CONSTANT = -1 - - -def _make_vehicle_observation(road_map, neighborhood_vehicle): - nv_lane = road_map.nearest_lane(neighborhood_vehicle.pose.point, radius=3) - if nv_lane: - nv_road_id = nv_lane.road.road_id - nv_lane_id = nv_lane.lane_id - nv_lane_index = nv_lane.index - else: - nv_road_id = ROAD_ID_CONSTANT - nv_lane_id = LANE_ID_CONSTANT - nv_lane_index = LANE_INDEX_CONSTANT - - return VehicleObservation( - id=neighborhood_vehicle.actor_id, - position=neighborhood_vehicle.pose.position, - bounding_box=neighborhood_vehicle.dimensions, - heading=neighborhood_vehicle.pose.heading, - speed=neighborhood_vehicle.speed, - road_id=nv_road_id, - lane_id=nv_lane_id, - lane_index=nv_lane_index, - lane_position=None, - ) - - -class Sensors: - """Sensor utility""" - - _log = logging.getLogger("Sensors") - - @staticmethod - def observe_batch( - sim, agent_id, sensor_states, vehicles - ) -> Tuple[Dict[str, Observation], Dict[str, bool]]: - """Operates all sensors on a batch of vehicles for a single agent.""" - # TODO: Replace this with a more efficient implementation that _actually_ - # does batching - assert sensor_states.keys() == vehicles.keys() - - observations, dones = {}, {} - for vehicle_id, vehicle in vehicles.items(): - sensor_state = sensor_states[vehicle_id] - observations[vehicle_id], dones[vehicle_id] = Sensors.observe( - sim, agent_id, sensor_state, vehicle - ) - - return observations, dones - - @staticmethod - def observe(sim, agent_id, sensor_state, vehicle) -> Tuple[Observation, bool]: - """Generate observations for the given agent around the given vehicle.""" - neighborhood_vehicle_states = None - if vehicle.subscribed_to_neighborhood_vehicle_states_sensor: - neighborhood_vehicle_states = [] - for nv in vehicle.neighborhood_vehicle_states_sensor(): - veh_obs = _make_vehicle_observation(sim.road_map, nv) - nv_lane_pos = None - if ( - veh_obs.lane_id is not LANE_ID_CONSTANT - and vehicle.subscribed_to_lane_position_sensor - ): - nv_lane_pos = vehicle.lane_position_sensor( - sim.road_map.lane_by_id(veh_obs.lane_id), nv - ) - neighborhood_vehicle_states.append( - veh_obs._replace(lane_position=nv_lane_pos) - ) - - if vehicle.subscribed_to_waypoints_sensor: - waypoint_paths = vehicle.waypoints_sensor() - else: - waypoint_paths = sim.road_map.waypoint_paths( - vehicle.pose, - lookahead=1, - within_radius=vehicle.length, - ) - - closest_lane = sim.road_map.nearest_lane(vehicle.pose.point) - ego_lane_pos = None - if closest_lane: - ego_lane_id = closest_lane.lane_id - ego_lane_index = closest_lane.index - ego_road_id = closest_lane.road.road_id - if vehicle.subscribed_to_lane_position_sensor: - ego_lane_pos = vehicle.lane_position_sensor(closest_lane, vehicle) - else: - ego_lane_id = LANE_ID_CONSTANT - ego_lane_index = LANE_INDEX_CONSTANT - ego_road_id = ROAD_ID_CONSTANT - ego_vehicle_state = vehicle.state - - acceleration_params = { - "linear_acceleration": None, - "angular_acceleration": None, - "linear_jerk": None, - "angular_jerk": None, - } - if vehicle.subscribed_to_accelerometer_sensor: - acceleration_values = vehicle.accelerometer_sensor( - ego_vehicle_state.linear_velocity, - ego_vehicle_state.angular_velocity, - sim.last_dt, - ) - acceleration_params.update( - dict( - zip( - [ - "linear_acceleration", - "angular_acceleration", - "linear_jerk", - "angular_jerk", - ], - acceleration_values, - ) - ) - ) - - ego_vehicle = EgoVehicleObservation( - id=ego_vehicle_state.actor_id, - position=ego_vehicle_state.pose.point.as_np_array, - bounding_box=ego_vehicle_state.dimensions, - heading=Heading(ego_vehicle_state.pose.heading), - speed=ego_vehicle_state.speed, - steering=ego_vehicle_state.steering, - yaw_rate=ego_vehicle_state.yaw_rate, - road_id=ego_road_id, - lane_id=ego_lane_id, - lane_index=ego_lane_index, - mission=sensor_state.plan.mission, - linear_velocity=ego_vehicle_state.linear_velocity, - angular_velocity=ego_vehicle_state.angular_velocity, - lane_position=ego_lane_pos, - **acceleration_params, - ) - - road_waypoints = ( - vehicle.road_waypoints_sensor() - if vehicle.subscribed_to_road_waypoints_sensor - else None - ) - - near_via_points = [] - hit_via_points = [] - if vehicle.subscribed_to_via_sensor: - ( - near_via_points, - hit_via_points, - ) = vehicle.via_sensor() - via_data = Vias( - near_via_points=near_via_points, - hit_via_points=hit_via_points, - ) - - if waypoint_paths: - vehicle.trip_meter_sensor.update_distance_wps_record(waypoint_paths) - distance_travelled = vehicle.trip_meter_sensor(sim) - - vehicle.driven_path_sensor.track_latest_driven_path(sim) - - if not vehicle.subscribed_to_waypoints_sensor: - waypoint_paths = None - - drivable_area_grid_map = ( - vehicle.drivable_area_grid_map_sensor() - if vehicle.subscribed_to_drivable_area_grid_map_sensor - else None - ) - occupancy_grid_map = ( - vehicle.ogm_sensor() if vehicle.subscribed_to_ogm_sensor else None - ) - top_down_rgb = ( - vehicle.rgb_sensor() if vehicle.subscribed_to_rgb_sensor else None - ) - lidar_point_cloud = ( - vehicle.lidar_sensor() if vehicle.subscribed_to_lidar_sensor else None - ) - - done, events = Sensors._is_done_with_events( - sim, agent_id, vehicle, sensor_state - ) - - if done and sensor_state.steps_completed == 1: - agent_type = "Social agent" - if agent_id in sim.agent_manager.ego_agent_ids: - agent_type = "Ego agent" - logger.warning( - f"{agent_type} with Agent ID: {agent_id} is done on the first step" - ) - - signals = None - if vehicle.subscribed_to_signals_sensor: - provider_state = sim._last_provider_state - signals = vehicle.signals_sensor( - closest_lane, - ego_lane_pos, - ego_vehicle_state, - sensor_state.plan, - provider_state, - ) - - agent_controls = agent_id == sim.vehicle_index.actor_id_from_vehicle_id( - ego_vehicle_state.actor_id - ) - - return ( - Observation( - dt=sim.last_dt, - step_count=sim.step_count, - steps_completed=sensor_state.steps_completed, - elapsed_sim_time=sim.elapsed_sim_time, - events=events, - ego_vehicle_state=ego_vehicle, - under_this_agent_control=agent_controls, - neighborhood_vehicle_states=neighborhood_vehicle_states, - waypoint_paths=waypoint_paths, - distance_travelled=distance_travelled, - top_down_rgb=top_down_rgb, - occupancy_grid_map=occupancy_grid_map, - drivable_area_grid_map=drivable_area_grid_map, - lidar_point_cloud=lidar_point_cloud, - road_waypoints=road_waypoints, - via_data=via_data, - signals=signals, - ), - done, - ) - - @staticmethod - def step(sim, sensor_state): - """Step the sensor state.""" - return sensor_state.step() - - @classmethod - def _agents_alive_done_check( - cls, agent_manager, agents_alive: Optional[AgentsAliveDoneCriteria] - ): - if agents_alive is None: - return False - - if ( - agents_alive.minimum_ego_agents_alive - and len(agent_manager.ego_agent_ids) < agents_alive.minimum_ego_agents_alive - ): - return True - if ( - agents_alive.minimum_total_agents_alive - and len(agent_manager.agent_ids) < agents_alive.minimum_total_agents_alive - ): - return True - if agents_alive.agent_lists_alive: - for agents_list_alive in agents_alive.agent_lists_alive: - assert isinstance( - agents_list_alive.agents_list, (List, Set, Tuple) - ), "Please specify a list of agent ids to watch" - assert isinstance( - agents_list_alive.minimum_agents_alive_in_list, int - ), "Please specify an int for minimum number of alive agents in the list" - assert ( - agents_list_alive.minimum_agents_alive_in_list >= 0 - ), "minimum_agents_alive_in_list should not be negative" - agents_alive_check = [ - 1 if id in agent_manager.agent_ids else 0 - for id in agents_list_alive.agents_list - ] - if ( - agents_alive_check.count(1) - < agents_list_alive.minimum_agents_alive_in_list - ): - return True - - return False - - @classmethod - def _actors_alive_done_check( - cls, - vehicle_index, - sensor_state, - actors_alive: Optional[ActorsAliveDoneCriteria], - ): - if actors_alive is None: - return False - - sensor_state: SensorState = sensor_state - from smarts.core.vehicle_index import VehicleIndex - - vehicle_index: VehicleIndex = vehicle_index - - pattern = re.compile( - "|".join(rf"(?:{aoi})" for aoi in actors_alive.actors_of_interest) - ) - ## TODO optimization to get vehicles that were added and removed last step - ## TODO second optimization to check for already known vehicles - for vehicle_id in vehicle_index.vehicle_ids(): - # get vehicles by pattern - if pattern.match(vehicle_id): - sensor_state.seen_interest_actors = True - return False - if actors_alive.strict or sensor_state.seen_interest_actors: - # if agent requires the actor to exist immediately - # OR if previously seen relevant actors but no actors match anymore - return True - - ## if never seen a relevant actor - return False - - @classmethod - def _is_done_with_events(cls, sim, agent_id, vehicle, sensor_state): - interface = sim.agent_manager.agent_interface_for_agent_id(agent_id) - done_criteria = interface.done_criteria - event_config = interface.event_configuration - - # TODO: the following calls nearest_lanes (expensive) 6 times - reached_goal = cls._agent_reached_goal(sim, vehicle) - collided = sim.vehicle_did_collide(vehicle.id) - is_off_road = cls._vehicle_is_off_road(sim, vehicle) - is_on_shoulder = cls._vehicle_is_on_shoulder(sim, vehicle) - is_not_moving = cls._vehicle_is_not_moving( - sim, vehicle, event_config.not_moving_time, event_config.not_moving_distance - ) - reached_max_episode_steps = sensor_state.reached_max_episode_steps - is_off_route, is_wrong_way = cls._vehicle_is_off_route_and_wrong_way( - sim, vehicle - ) - agents_alive_done = cls._agents_alive_done_check( - sim.agent_manager, done_criteria.agents_alive - ) - actors_alive_done = cls._actors_alive_done_check( - sim.vehicle_index, sensor_state, done_criteria.actors_alive - ) - - done = not sim.resetting and ( - (is_off_road and done_criteria.off_road) - or reached_goal - or reached_max_episode_steps - or (is_on_shoulder and done_criteria.on_shoulder) - or (collided and done_criteria.collision) - or (is_not_moving and done_criteria.not_moving) - or (is_off_route and done_criteria.off_route) - or (is_wrong_way and done_criteria.wrong_way) - or agents_alive_done - or actors_alive_done - ) - - events = Events( - collisions=sim.vehicle_collisions(vehicle.id), - off_road=is_off_road, - reached_goal=reached_goal, - reached_max_episode_steps=reached_max_episode_steps, - off_route=is_off_route, - on_shoulder=is_on_shoulder, - wrong_way=is_wrong_way, - not_moving=is_not_moving, - agents_alive_done=agents_alive_done, - actors_alive_done=actors_alive_done, - ) - - return done, events - - @classmethod - def _agent_reached_goal(cls, sim, vehicle): - sensor_state = sim.vehicle_index.sensor_state_for_vehicle_id(vehicle.id) - distance_travelled = vehicle.trip_meter_sensor() - mission = sensor_state.plan.mission - return mission.is_complete(vehicle, distance_travelled) - - @classmethod - def _vehicle_is_off_road(cls, sim, vehicle): - return not sim.scenario.road_map.road_with_point(vehicle.pose.point) - - @classmethod - def _vehicle_is_on_shoulder(cls, sim, vehicle): - # XXX: this isn't technically right as this would also return True - # for vehicles that are completely off road. - for corner_coordinate in vehicle.bounding_box: - if not sim.scenario.road_map.road_with_point(Point(*corner_coordinate)): - return True - return False - - @classmethod - def _vehicle_is_not_moving( - cls, sim, vehicle, last_n_seconds_considered, min_distance_moved - ): - # Flag if the vehicle has been immobile for the past 'last_n_seconds_considered' seconds - if sim.elapsed_sim_time < last_n_seconds_considered: - return False - - distance = vehicle.driven_path_sensor.distance_travelled( - sim, last_n_seconds=last_n_seconds_considered - ) - - # Due to controller instabilities there may be some movement even when a - # vehicle is "stopped". - return distance < min_distance_moved - - @classmethod - def _vehicle_is_off_route_and_wrong_way(cls, sim, vehicle): - """Determines if the agent is on route and on the correct side of the road. - - Args: - sim: An instance of the simulator. - agent_id: The id of the agent to check. - - Returns: - A tuple (is_off_route, is_wrong_way) - is_off_route: - Actor's vehicle is not on its route or an oncoming traffic lane. - is_wrong_way: - Actor's vehicle is going against the lane travel direction. - """ - - sensor_state = sim.vehicle_index.sensor_state_for_vehicle_id(vehicle.id) - route_roads = sensor_state.plan.route.roads - - vehicle_pos = vehicle.pose.point - vehicle_minimum_radius_bounds = ( - np.linalg.norm(vehicle.chassis.dimensions.as_lwh[:2]) * 0.5 - ) - # Check that center of vehicle is still close to route - radius = vehicle_minimum_radius_bounds + 5 - nearest_lane = sim.scenario.road_map.nearest_lane(vehicle_pos, radius=radius) - - # No road nearby, so we're not on route! - if not nearest_lane: - return (True, False) - - # Check whether vehicle is in wrong-way - is_wrong_way = cls._check_wrong_way_event(nearest_lane, vehicle) - - # Check whether vehicle has no-route or is on-route - if ( - not route_roads # Vehicle has no-route. E.g., endless mission with a random route - or nearest_lane.road in route_roads # Vehicle is on-route - or nearest_lane.in_junction - ): - return (False, is_wrong_way) - - veh_offset = nearest_lane.offset_along_lane(vehicle_pos) - - # so we're obviously not on the route, but we might have just gone - # over the center line into an oncoming lane... - for on_lane in nearest_lane.oncoming_lanes_at_offset(veh_offset): - if on_lane.road in route_roads: - return (False, is_wrong_way) - - # Check for case if there was an early merge into another incoming lane. This means the - # vehicle should still be following the lane direction to be valid as still on route. - if not is_wrong_way: - # See if the lane leads into the current route - for lane in nearest_lane.outgoing_lanes: - if lane.road in route_roads: - return (False, is_wrong_way) - # If outgoing lane is in a junction see if the junction lane leads into current route. - if lane.in_junction: - for out_lane in lane.outgoing_lanes: - if out_lane.road in route_roads: - return (False, is_wrong_way) - - # Vehicle is completely off-route - return (True, is_wrong_way) - - @staticmethod - def _vehicle_is_wrong_way(vehicle, closest_lane): - target_pose = closest_lane.center_pose_at_point(vehicle.pose.point) - # Check if the vehicle heading is oriented away from the lane heading. - return ( - np.fabs(vehicle.pose.heading.relative_to(target_pose.heading)) > 0.5 * np.pi - ) - - @classmethod - def _check_wrong_way_event(cls, lane_to_check, vehicle): - # When the vehicle is in an intersection, turn off the `wrong way` check to avoid - # false positive `wrong way` events. - if lane_to_check.in_junction: - return False - return cls._vehicle_is_wrong_way(vehicle, lane_to_check) - - -class Sensor: - """The sensor base class.""" - - def step(self): - """Update sensor state.""" - pass - - def teardown(self): - """Clean up internal resources""" - raise NotImplementedError - - -class SensorState: - """Sensor state information""" - - def __init__(self, max_episode_steps: int, plan): - self._max_episode_steps = max_episode_steps - self._plan = plan - self._step = 0 - self._seen_interest_actors = False - - def step(self): - """Update internal state.""" - self._step += 1 - - @property - def seen_interest_actors(self) -> bool: - """If a relevant actor has been spotted before.""" - return self._seen_interest_actors - - @seen_interest_actors.setter - def seen_interest_actors(self, value: bool): - self._seen_interest_actors = value - - @property - def reached_max_episode_steps(self) -> bool: - """Inbuilt sensor information that describes if episode step limit has been reached.""" - if self._max_episode_steps is None: - return False - - return self._step >= self._max_episode_steps - - @property - def plan(self): - """Get the current plan for the actor.""" - return self._plan - - @property - def steps_completed(self) -> int: - """Get the number of steps where this sensor has been updated.""" - return self._step - - -class CameraSensor(Sensor): - """The base for a sensor that renders images.""" - - def __init__( - self, - vehicle, - renderer, # type Renderer or None - name: str, - mask: int, - width: int, - height: int, - resolution: float, - ): - assert renderer - self._log = logging.getLogger(self.__class__.__name__) - self._vehicle = vehicle - self._camera = renderer.build_offscreen_camera( - name, - mask, - width, - height, - resolution, - ) - self._follow_vehicle() # ensure we have a correct initial camera position - - def teardown(self): - self._camera.teardown() - - def step(self): - self._follow_vehicle() - - def _follow_vehicle(self): - largest_dim = max(self._vehicle._chassis.dimensions.as_lwh) - self._camera.update(self._vehicle.pose, 20 * largest_dim) - - -class DrivableAreaGridMapSensor(CameraSensor): - """A sensor that renders drivable area from around its target actor.""" - - def __init__( - self, - vehicle, - width: int, - height: int, - resolution: float, - renderer, # type Renderer or None - ): - super().__init__( - vehicle, - renderer, - "drivable_area_grid_map", - RenderMasks.DRIVABLE_AREA_HIDE, - width, - height, - resolution, - ) - self._resolution = resolution - - def __call__(self) -> DrivableAreaGridMap: - assert ( - self._camera is not None - ), "Drivable area grid map has not been initialized" - - ram_image = self._camera.wait_for_ram_image(img_format="A") - mem_view = memoryview(ram_image) - image = np.frombuffer(mem_view, np.uint8) - image.shape = (self._camera.tex.getYSize(), self._camera.tex.getXSize(), 1) - image = np.flipud(image) - - metadata = GridMapMetadata( - created_at=int(time.time()), - resolution=self._resolution, - height=image.shape[0], - width=image.shape[1], - camera_position=self._camera.camera_np.getPos(), - camera_heading_in_degrees=self._camera.camera_np.getH(), - ) - return DrivableAreaGridMap(data=image, metadata=metadata) - - -class OGMSensor(CameraSensor): - """A sensor that renders occupancy information from around its target actor.""" - - def __init__( - self, - vehicle, - width: int, - height: int, - resolution: float, - renderer, # type Renderer or None - ): - super().__init__( - vehicle, - renderer, - "ogm", - RenderMasks.OCCUPANCY_HIDE, - width, - height, - resolution, - ) - self._resolution = resolution - - def __call__(self) -> OccupancyGridMap: - assert self._camera is not None, "OGM has not been initialized" - - ram_image = self._camera.wait_for_ram_image(img_format="A") - mem_view = memoryview(ram_image) - grid = np.frombuffer(mem_view, np.uint8) - grid.shape = (self._camera.tex.getYSize(), self._camera.tex.getXSize(), 1) - grid = np.flipud(grid) - - metadata = GridMapMetadata( - created_at=int(time.time()), - resolution=self._resolution, - height=grid.shape[0], - width=grid.shape[1], - camera_position=self._camera.camera_np.getPos(), - camera_heading_in_degrees=self._camera.camera_np.getH(), - ) - return OccupancyGridMap(data=grid, metadata=metadata) - - -class RGBSensor(CameraSensor): - """A sensor that renders color values from around its target actor.""" - - def __init__( - self, - vehicle, - width: int, - height: int, - resolution: float, - renderer, # type Renderer or None - ): - super().__init__( - vehicle, - renderer, - "top_down_rgb", - RenderMasks.RGB_HIDE, - width, - height, - resolution, - ) - self._resolution = resolution - - def __call__(self) -> TopDownRGB: - assert self._camera is not None, "RGB has not been initialized" - - ram_image = self._camera.wait_for_ram_image(img_format="RGB") - mem_view = memoryview(ram_image) - image = np.frombuffer(mem_view, np.uint8) - image.shape = (self._camera.tex.getYSize(), self._camera.tex.getXSize(), 3) - image = np.flipud(image) - - metadata = GridMapMetadata( - created_at=int(time.time()), - resolution=self._resolution, - height=image.shape[0], - width=image.shape[1], - camera_position=self._camera.camera_np.getPos(), - camera_heading_in_degrees=self._camera.camera_np.getH(), - ) - return TopDownRGB(data=image, metadata=metadata) - - -class LidarSensor(Sensor): - """A lidar sensor.""" - - def __init__( - self, - vehicle, - bullet_client, - sensor_params: Optional[SensorParams] = None, - lidar_offset=(0, 0, 1), - ): - self._vehicle = vehicle - self._bullet_client = bullet_client - self._lidar_offset = np.array(lidar_offset) - - self._lidar = Lidar( - self._vehicle.position + self._lidar_offset, - sensor_params, - self._bullet_client, - ) - - def step(self): - self._follow_vehicle() - - def _follow_vehicle(self): - self._lidar.origin = self._vehicle.position + self._lidar_offset - - def __call__(self): - return self._lidar.compute_point_cloud() - - def teardown(self): - pass - - -class DrivenPathSensor(Sensor): - """Tracks the driven path as a series of positions (regardless if the vehicle is - following the route or not). For performance reasons it only keeps the last - N=max_path_length path segments. - """ - - Entry = namedtuple("TimeAndPos", ["timestamp", "position"]) - - def __init__(self, vehicle, max_path_length: int = 500): - self._vehicle = vehicle - self._driven_path = deque(maxlen=max_path_length) - - def track_latest_driven_path(self, sim): - """Records the current location of the tracked vehicle.""" - pos = self._vehicle.position[:2] - self._driven_path.append( - DrivenPathSensor.Entry(timestamp=sim.elapsed_sim_time, position=pos) - ) - - def __call__(self, count=sys.maxsize): - return [x.position for x in self._driven_path][-count:] - - def teardown(self): - pass - - def distance_travelled( - self, - sim, - last_n_seconds: Optional[float] = None, - last_n_steps: Optional[int] = None, - ): - """Find the amount of distance travelled over the last # of seconds XOR steps""" - if last_n_seconds is None and last_n_steps is None: - raise ValueError("Either last N seconds or last N steps must be provided") - - if last_n_steps is not None: - n = last_n_steps + 1 # to factor in the current step we're on - filtered_pos = [x.position for x in self._driven_path][-n:] - else: # last_n_seconds - threshold = sim.elapsed_sim_time - last_n_seconds - filtered_pos = [ - x.position for x in self._driven_path if x.timestamp >= threshold - ] - - xs = np.array([p[0] for p in filtered_pos]) - ys = np.array([p[1] for p in filtered_pos]) - dist_array = (xs[:-1] - xs[1:]) ** 2 + (ys[:-1] - ys[1:]) ** 2 - return np.sum(np.sqrt(dist_array)) - - -class TripMeterSensor(Sensor): - """Tracks distance travelled along the route (in meters). Meters driven while - off-route are not counted as part of the total. - """ - - def __init__(self, vehicle, road_map, plan): - self._vehicle = vehicle - self._road_map: RoadMap = road_map - self._plan = plan - self._wps_for_distance: List[Waypoint] = [] - self._dist_travelled = 0.0 - self._last_dist_travelled = 0.0 - self._last_actor_position = None - - def update_distance_wps_record(self, waypoint_paths): - """Append a waypoint to the history if it is not already counted.""" - # Distance calculation. Intention is the shortest trip travelled at the lane - # level the agent has travelled. This is to prevent lateral movement from - # increasing the total distance travelled. - self._last_dist_travelled = self._dist_travelled - - new_wp = waypoint_paths[0][0] - wp_road = self._road_map.lane_by_id(new_wp.lane_id).road.road_id - - should_count_wp = ( - # if we do not have a fixed route, we count all waypoints we accumulate - not self._plan.mission.requires_route - # if we have a route to follow, only count wps on route - or wp_road in [road.road_id for road in self._plan.route.roads] - ) - - if not self._wps_for_distance: - self._last_actor_position = self._vehicle.pose.position - if should_count_wp: - self._wps_for_distance.append(new_wp) - return # sensor does not have enough history - most_recent_wp = self._wps_for_distance[-1] - - # TODO: Instead of storing a waypoint every 0.5m just find the next one immediately - threshold_for_counting_wp = 0.5 # meters from last tracked waypoint - if ( - np.linalg.norm(new_wp.pos - most_recent_wp.pos) > threshold_for_counting_wp - and should_count_wp - ): - self._wps_for_distance.append(new_wp) - additional_distance = TripMeterSensor._compute_additional_dist_travelled( - most_recent_wp, - new_wp, - self._vehicle.pose.position, - self._last_actor_position, - ) - self._dist_travelled += additional_distance - self._last_actor_position = self._vehicle.pose.position - - @staticmethod - def _compute_additional_dist_travelled( - recent_wp: Waypoint, - new_waypoint: Waypoint, - vehicle_position: np.ndarray, - last_vehicle_pos: np.ndarray, - ): - # old waypoint minus current ahead waypoint - wp_disp_vec = new_waypoint.pos - recent_wp.pos - # make unit vector - wp_unit_vec = wp_disp_vec / (np.linalg.norm(wp_disp_vec) or 1) - # vehicle position minus last vehicle position - position_disp_vec = vehicle_position[:2] - last_vehicle_pos[:2] - # distance of vehicle between last and current wp - distance = np.dot(position_disp_vec, wp_unit_vec) - return distance - - def __call__(self, increment=False): - if increment: - return self._dist_travelled - self._last_dist_travelled - - return self._dist_travelled - - def teardown(self): - pass - - -class NeighborhoodVehiclesSensor(Sensor): - """Detects other vehicles around the sensor equipped vehicle.""" - - def __init__(self, vehicle, sim, radius=None): - self._vehicle = vehicle - self._sim = weakref.ref(sim) - self._radius = radius - - @property - def radius(self): - """Radius to check for nearby vehicles.""" - return self._radius - - def __call__(self): - sim = self._sim() - assert sim - return sim.neighborhood_vehicles_around_vehicle( - self._vehicle, radius=self._radius - ) - - def teardown(self): - pass - - -class WaypointsSensor(Sensor): - """Detects waypoints leading forward along the vehicle plan.""" - - def __init__(self, vehicle, plan: Plan, lookahead=32): - self._vehicle = vehicle - self._plan = plan - self._lookahead = lookahead - - def __call__(self): - return self._plan.road_map.waypoint_paths( - self._vehicle.pose, - lookahead=self._lookahead, - route=self._plan.route, - ) - - def teardown(self): - pass - - -class RoadWaypointsSensor(Sensor): - """Detects waypoints from all paths nearby the vehicle.""" - - def __init__(self, vehicle, sim, plan, horizon=32): - self._vehicle = vehicle - self._road_map = sim.road_map - self._plan = plan - self._horizon = horizon - - def __call__(self) -> RoadWaypoints: - veh_pt = self._vehicle.pose.point - lane = self._road_map.nearest_lane(veh_pt) - if not lane: - return RoadWaypoints(lanes={}) - road = lane.road - lane_paths = {} - for croad in ( - [road] + road.parallel_roads + road.oncoming_roads_at_point(veh_pt) - ): - for lane in croad.lanes: - lane_paths[lane.lane_id] = self.paths_for_lane(lane) - - return RoadWaypoints(lanes=lane_paths) - - def paths_for_lane(self, lane, overflow_offset=None): - """Gets waypoint paths along the given lane.""" - # XXX: the following assumes waypoint spacing is 1m - if overflow_offset is None: - offset = lane.offset_along_lane(self._vehicle.pose.point) - start_offset = offset - self._horizon - else: - start_offset = lane.length + overflow_offset - - incoming_lanes = lane.incoming_lanes - if start_offset < 0 and len(incoming_lanes) > 0: - paths = [] - for lane in incoming_lanes: - paths += self.paths_for_lane(lane, start_offset) - return paths - else: - start_offset = max(0, start_offset) - wp_start = lane.from_lane_coord(RefLinePoint(start_offset)) - adj_pose = Pose.from_center(wp_start, self._vehicle.heading) - wps_to_lookahead = self._horizon * 2 - paths = lane.waypoint_paths_for_pose( - pose=adj_pose, - lookahead=wps_to_lookahead, - route=self._plan.route, - ) - return paths - - def teardown(self): - pass - - -class AccelerometerSensor(Sensor): - """Tracks motion changes within the vehicle equipped with this sensor.""" - - def __init__(self, vehicle): - self.linear_velocities = deque(maxlen=3) - self.angular_velocities = deque(maxlen=3) - - def __call__(self, linear_velocity, angular_velocity, dt: float): - if linear_velocity is not None: - self.linear_velocities.append(linear_velocity) - if angular_velocity is not None: - self.angular_velocities.append(angular_velocity) - - linear_acc = np.array((0.0, 0.0, 0.0)) - angular_acc = np.array((0.0, 0.0, 0.0)) - linear_jerk = np.array((0.0, 0.0, 0.0)) - angular_jerk = np.array((0.0, 0.0, 0.0)) - - if not dt: - return (linear_acc, angular_acc, linear_jerk, angular_jerk) - - if len(self.linear_velocities) >= 2: - linear_acc = (self.linear_velocities[-1] - self.linear_velocities[-2]) / dt - if len(self.linear_velocities) >= 3: - last_linear_acc = ( - self.linear_velocities[-2] - self.linear_velocities[-3] - ) / dt - linear_jerk = linear_acc - last_linear_acc - if len(self.angular_velocities) >= 2: - angular_acc = ( - self.angular_velocities[-1] - self.angular_velocities[-2] - ) / dt - if len(self.angular_velocities) >= 3: - last_angular_acc = ( - self.angular_velocities[-2] - self.angular_velocities[-3] - ) / dt - angular_jerk = angular_acc - last_angular_acc - - return (linear_acc, angular_acc, linear_jerk, angular_jerk) - - def teardown(self): - pass - - -class LanePositionSensor(Sensor): - """Tracks lane-relative RefLine (Frenet) coordinates.""" - - def __init__(self, vehicle): - pass - - def __call__(self, lane: RoadMap.Lane, vehicle): - return lane.to_lane_coord(vehicle.pose.point) - - def teardown(self): - pass - - -class ViaSensor(Sensor): - """Tracks collection of ViaPoint collectables""" - - def __init__(self, vehicle, plan, lane_acquisition_range, speed_accuracy): - self._consumed_via_points = set() - self._plan: Plan = plan - self._acquisition_range = lane_acquisition_range - self._vehicle = vehicle - self._speed_accuracy = speed_accuracy - - @property - def _vias(self) -> Iterable[Via]: - return self._plan.mission.via - - def __call__(self): - near_points: List[ViaPoint] = list() - hit_points: List[ViaPoint] = list() - vehicle_position = self._vehicle.position[:2] - - @lru_cache() - def closest_point_on_lane(position, lane_id): - lane = self._plan.road_map.lane_by_id(lane_id) - return lane.center_at_point(position) - - for via in self._vias: - closest_position_on_lane = closest_point_on_lane( - tuple(vehicle_position), via.lane_id - ) - closest_position_on_lane = closest_position_on_lane[:2] - - dist_from_lane_sq = squared_dist(vehicle_position, closest_position_on_lane) - if dist_from_lane_sq > self._acquisition_range**2: - continue - - point = ViaPoint( - tuple(via.position), - lane_index=via.lane_index, - road_id=via.road_id, - required_speed=via.required_speed, - ) - - near_points.append(point) - dist_from_point_sq = squared_dist(vehicle_position, via.position) - if ( - dist_from_point_sq <= via.hit_distance**2 - and via not in self._consumed_via_points - and np.isclose( - self._vehicle.speed, via.required_speed, atol=self._speed_accuracy - ) - ): - self._consumed_via_points.add(via) - hit_points.append(point) - - return ( - sorted( - near_points, - key=lambda point: squared_dist(point.position, vehicle_position), - ), - hit_points, - ) - - def teardown(self): - pass - - -class SignalsSensor(Sensor): - """Reports state of traffic signals (lights) in the lanes ahead of vehicle.""" - - def __init__(self, vehicle, road_map: RoadMap, lookahead: float): - self._logger = logging.getLogger(self.__class__.__name__) - self._vehicle = vehicle - self._road_map = road_map - self._lookahead = lookahead - - @staticmethod - def _is_signal_type(feature: RoadMap.Feature) -> bool: - # XXX: eventually if we add other types of dynamic features, we'll need to update this. - return ( - feature.type == RoadMap.FeatureType.FIXED_LOC_SIGNAL or feature.is_dynamic - ) - - def __call__( - self, - lane: Optional[RoadMap.Lane], - lane_pos: RefLinePoint, - state, # VehicleState - plan: Plan, - provider_state, # ProviderState - ) -> List[SignalObservation]: - result = [] - if not lane: - return result - upcoming_signals = [] - for feat in lane.features: - if not self._is_signal_type(feat): - continue - for pt in feat.geometry: - # TAI: we might want to use the position of our back bumper - # instead of centroid to allow agents to have some (even more) - # imprecision in their handling of stopping at signals. - if lane.offset_along_lane(pt) >= lane_pos.s: - upcoming_signals.append(feat) - break - lookahead = self._lookahead - lane.length + lane_pos.s - self._find_signals_ahead(lane, lookahead, plan.route, upcoming_signals) - - for signal in upcoming_signals: - for actor_state in provider_state.actors: - if actor_state.actor_id == signal.feature_id: - signal_state = actor_state - break - else: - self._logger.warning( - "could not find signal state corresponding with feature_id={signal.feature_id}" - ) - continue - assert isinstance(signal_state, SignalState) - controlled_lanes = None - if signal_state.controlled_lanes: - controlled_lanes = [cl.lane_id for cl in signal_state.controlled_lanes] - result.append( - SignalObservation( - state=signal_state.state, - stop_point=signal_state.stopping_pos, - controlled_lanes=controlled_lanes, - last_changed=signal_state.last_changed, - ) - ) - - return result - - def _find_signals_ahead( - self, - lane: RoadMap.Lane, - lookahead: float, - route: Optional[RoadMap.Route], - upcoming_signals: List[RoadMap.Feature], - ): - if lookahead <= 0: - return - for ogl in lane.outgoing_lanes: - if route and route.road_length > 0 and ogl.road not in route.roads: - continue - upcoming_signals += [ - feat for feat in ogl.features if self._is_signal_type(feat) - ] - self._find_signals_ahead( - ogl, lookahead - lane.length, route, upcoming_signals - ) - - def teardown(self): - pass diff --git a/smarts/core/sensors/__init__.py b/smarts/core/sensors/__init__.py new file mode 100644 index 0000000000..e94eab059f --- /dev/null +++ b/smarts/core/sensors/__init__.py @@ -0,0 +1,746 @@ +# Copyright (C) 2020. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging +import re +import sys +from typing import Any, Dict, List, Optional, Set, Tuple + +import numpy as np + +from smarts.core.agent_interface import ActorsAliveDoneCriteria, AgentsAliveDoneCriteria +from smarts.core.coordinates import Heading, Point +from smarts.core.events import Events +from smarts.core.observations import ( + DrivableAreaGridMap, + EgoVehicleObservation, + GridMapMetadata, + Observation, + OccupancyGridMap, + RoadWaypoints, + SignalObservation, + TopDownRGB, + VehicleObservation, + ViaPoint, + Vias, +) +from smarts.core.plan import Plan, PlanFrame +from smarts.core.road_map import RoadMap +from smarts.core.sensor import ( + AccelerometerSensor, + CameraSensor, + DrivableAreaGridMapSensor, + DrivenPathSensor, + LanePositionSensor, + LidarSensor, + NeighborhoodVehiclesSensor, + OGMSensor, + RGBSensor, + RoadWaypointsSensor, + Sensor, + SignalsSensor, + TripMeterSensor, + ViaSensor, + WaypointsSensor, +) +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.vehicle_state import VehicleState + +logger = logging.getLogger(__name__) + +LANE_ID_CONSTANT = "off_lane" +ROAD_ID_CONSTANT = "off_road" +LANE_INDEX_CONSTANT = -1 + + +def _make_vehicle_observation(road_map, neighborhood_vehicle): + nv_lane = road_map.nearest_lane(neighborhood_vehicle.pose.point, radius=3) + if nv_lane: + nv_road_id = nv_lane.road.road_id + nv_lane_id = nv_lane.lane_id + nv_lane_index = nv_lane.index + else: + nv_road_id = ROAD_ID_CONSTANT + nv_lane_id = LANE_ID_CONSTANT + nv_lane_index = LANE_INDEX_CONSTANT + + return VehicleObservation( + id=neighborhood_vehicle.actor_id, + position=neighborhood_vehicle.pose.position, + bounding_box=neighborhood_vehicle.dimensions, + heading=neighborhood_vehicle.pose.heading, + speed=neighborhood_vehicle.speed, + road_id=nv_road_id, + lane_id=nv_lane_id, + lane_index=nv_lane_index, + lane_position=None, + ) + + +class SensorState: + """Sensor state information""" + + def __init__(self, max_episode_steps: int, plan_frame: PlanFrame): + self._max_episode_steps = max_episode_steps + self._plan_frame = plan_frame + self._step = 0 + self._seen_interest_actors = False + + def step(self): + """Update internal state.""" + self._step += 1 + + @property + def seen_interest_actors(self) -> bool: + """If a relevant actor has been spotted before.""" + return self._seen_interest_actors + + @seen_interest_actors.setter + def seen_interest_actors(self, value: bool): + self._seen_interest_actors = value + + @property + def reached_max_episode_steps(self) -> bool: + """Inbuilt sensor information that describes if episode step limit has been reached.""" + if self._max_episode_steps is None: + return False + + return self._step >= self._max_episode_steps + + def get_plan(self, road_map: RoadMap): + """Get the current plan for the actor.""" + return Plan.from_frame(self._plan_frame, road_map) + + @property + def steps_completed(self) -> int: + """Get the number of steps where this sensor has been updated.""" + return self._step + + +class SensorResolver: + """An interface describing sensor observation and update systems.""" + + # TODO: Remove renderer and bullet client from the arguments + def observe( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], + renderer, + bullet_client, + ): + """Generate observations + + Args: + sim_frame (SimulationFrame): The simulation frame. + sim_local_constants (SimulationLocalConstants): Constraints defined by the local simulator. + agent_ids (Set[str]): The agents to run. + renderer (Renderer): The renderer to use. + bullet_client (Any): The bullet client. This parameter is likely to be removed. + """ + raise NotImplementedError() + + def step(self, sim_frame, sensor_states): + """Step the sensor state.""" + raise NotImplementedError() + + +class Sensors: + """Sensor related utilities""" + + @classmethod + def observe_serializable_sensor_batch( + cls, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids_for_group, + ): + """Run the serializable sensors in a batch.""" + observations, dones, updated_sensors = {}, {}, {} + for agent_id in agent_ids_for_group: + vehicle_ids = sim_frame.vehicles_for_agents.get(agent_id) + if not vehicle_ids: + continue + for vehicle_id in vehicle_ids: + ( + observations[agent_id], + dones[agent_id], + updated_sensors[vehicle_id], + ) = cls.process_serialization_safe_sensors( + sim_frame, + sim_local_constants, + agent_id, + sim_frame.sensor_states[vehicle_id], + vehicle_id, + ) + return observations, dones, updated_sensors + + @staticmethod + def process_serialization_unsafe_sensors( + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_id, + sensor_state, + vehicle_id, + renderer, + bullet_client, + ): + """Run observations that can only be done on the main thread.""" + vehicle_sensors: Dict[str, Any] = sim_frame.vehicle_sensors[vehicle_id] + + vehicle_state = sim_frame.vehicle_states[vehicle_id] + lidar = None + lidar_sensor = vehicle_sensors.get("lidar_sensor") + if lidar_sensor: + lidar_sensor.follow_vehicle(vehicle_state) + lidar = lidar_sensor(bullet_client) + + def get_camera_sensor_result(sensors, sensor_name, renderer): + return ( + sensors[sensor_name](renderer=renderer) + if renderer and sensors.get(sensor_name) + else None + ) + + updated_sensors = { + sensor_name: sensor + for sensor_name, sensor in vehicle_sensors.items() + if sensor.mutable and not sensor.serializable + } + + return ( + dict( + drivable_area_grid_map=get_camera_sensor_result( + vehicle_sensors, "drivable_area_grid_map_sensor", renderer + ), + occupancy_grid_map=get_camera_sensor_result( + vehicle_sensors, "ogm_sensor", renderer + ), + top_down_rgb=get_camera_sensor_result( + vehicle_sensors, "rgb_sensor", renderer + ), + lidar_point_cloud=lidar, + ), + updated_sensors, + ) + + @staticmethod + def process_serialization_safe_sensors( + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_id, + sensor_state, + vehicle_id, + ): + """Observations that can be done on any thread.""" + vehicle_sensors = sim_frame.vehicle_sensors[vehicle_id] + vehicle_state = sim_frame.vehicle_states[vehicle_id] + plan = sensor_state.get_plan(sim_local_constants.road_map) + neighborhood_vehicle_states = None + neighborhood_vehicle_states_sensor = vehicle_sensors.get( + "neighborhood_vehicle_states_sensor" + ) + if neighborhood_vehicle_states_sensor: + neighborhood_vehicle_states = [] + for nv in neighborhood_vehicle_states_sensor( + vehicle_state, sim_frame.vehicle_states.values() + ): + veh_obs = _make_vehicle_observation(sim_local_constants.road_map, nv) + lane_position_sensor = vehicle_sensors.get("lane_position_sensor") + nv_lane_pos = None + if veh_obs.lane_id is not LANE_ID_CONSTANT and lane_position_sensor: + nv_lane_pos = lane_position_sensor( + sim_local_constants.road_map.lane_by_id(veh_obs.lane_id), nv + ) + neighborhood_vehicle_states.append( + veh_obs._replace(lane_position=nv_lane_pos) + ) + + waypoints_sensor = vehicle_sensors.get("waypoints_sensor") + if waypoints_sensor: + waypoint_paths = waypoints_sensor( + vehicle_state, plan, sim_local_constants.road_map + ) + else: + waypoint_paths = sim_local_constants.road_map.waypoint_paths( + vehicle_state.pose, + lookahead=1, + within_radius=vehicle_state.dimensions.length, + ) + + closest_lane = sim_local_constants.road_map.nearest_lane( + vehicle_state.pose.point + ) + ego_lane_pos = None + if closest_lane: + ego_lane_id = closest_lane.lane_id + ego_lane_index = closest_lane.index + ego_road_id = closest_lane.road.road_id + lane_position_sensor = vehicle_sensors.get("lane_position_sensor") + if lane_position_sensor: + ego_lane_pos = lane_position_sensor(closest_lane, vehicle_state) + else: + ego_lane_id = LANE_ID_CONSTANT + ego_lane_index = LANE_INDEX_CONSTANT + ego_road_id = ROAD_ID_CONSTANT + + acceleration_params = { + "linear_acceleration": None, + "angular_acceleration": None, + "linear_jerk": None, + "angular_jerk": None, + } + + accelerometer_sensor = vehicle_sensors.get("accelerometer_sensor") + if accelerometer_sensor: + acceleration_values = accelerometer_sensor( + vehicle_state.linear_velocity, + vehicle_state.angular_velocity, + sim_frame.last_dt, + ) + acceleration_params.update( + dict( + zip( + [ + "linear_acceleration", + "angular_acceleration", + "linear_jerk", + "angular_jerk", + ], + acceleration_values, + ) + ) + ) + + ego_vehicle = EgoVehicleObservation( + id=vehicle_state.actor_id, + position=vehicle_state.pose.point.as_np_array, + bounding_box=vehicle_state.dimensions, + heading=Heading(vehicle_state.pose.heading), + speed=vehicle_state.speed, + steering=vehicle_state.steering, + yaw_rate=vehicle_state.yaw_rate, + road_id=ego_road_id, + lane_id=ego_lane_id, + lane_index=ego_lane_index, + mission=plan.mission, + linear_velocity=vehicle_state.linear_velocity, + angular_velocity=vehicle_state.angular_velocity, + lane_position=ego_lane_pos, + **acceleration_params, + ) + + road_waypoints_sensor = vehicle_sensors.get("road_waypoints_sensor") + road_waypoints = ( + road_waypoints_sensor(vehicle_state, plan, sim_local_constants.road_map) + if road_waypoints_sensor + else None + ) + + near_via_points = [] + hit_via_points = [] + + via_sensor = vehicle_sensors.get("via_sensor") + if via_sensor: + ( + near_via_points, + hit_via_points, + ) = via_sensor(vehicle_state, plan, sim_local_constants.road_map) + via_data = Vias( + near_via_points=near_via_points, + hit_via_points=hit_via_points, + ) + + distance_travelled = 0 + trip_meter_sensor: Optional[TripMeterSensor] = vehicle_sensors.get( + "trip_meter_sensor" + ) + if trip_meter_sensor: + if waypoint_paths: + trip_meter_sensor.update_distance_wps_record( + waypoint_paths, vehicle_state, plan, sim_local_constants.road_map + ) + distance_travelled = trip_meter_sensor(increment=True) + + driven_path_sensor = vehicle_sensors.get("driven_path_sensor") + if driven_path_sensor: + driven_path_sensor.track_latest_driven_path( + sim_frame.elapsed_sim_time, vehicle_state + ) + + if not waypoints_sensor: + waypoint_paths = None + + done, events = Sensors._is_done_with_events( + sim_frame, + sim_local_constants, + agent_id, + vehicle_state, + sensor_state, + plan, + vehicle_sensors, + ) + + if done and sensor_state.steps_completed == 1: + agent_type = "Social agent" + if agent_id in sim_frame.ego_ids: + agent_type = "Ego agent" + logger.warning( + "%s with Agent ID: %s is done on the first step", agent_type, agent_id + ) + + signals = None + signals_sensor = vehicle_sensors.get("signals_sensor") + if signals_sensor: + provider_state = sim_frame.last_provider_state + signals = signals_sensor( + closest_lane, + ego_lane_pos, + vehicle_state, + plan, + provider_state, + ) + + agent_controls = agent_id == sim_frame.agent_vehicle_controls.get( + vehicle_state.actor_id + ) + updated_sensors = { + sensor_name: sensor + for sensor_name, sensor in vehicle_sensors.items() + if sensor.mutable and sensor.serializable + } + + return ( + Observation( + dt=sim_frame.last_dt, + step_count=sim_frame.step_count, + steps_completed=sensor_state.steps_completed, + elapsed_sim_time=sim_frame.elapsed_sim_time, + events=events, + ego_vehicle_state=ego_vehicle, + under_this_agent_control=agent_controls, + neighborhood_vehicle_states=neighborhood_vehicle_states, + waypoint_paths=waypoint_paths, + distance_travelled=distance_travelled, + road_waypoints=road_waypoints, + via_data=via_data, + signals=signals, + ), + done, + updated_sensors, + ) + + @classmethod + def observe_vehicle( + cls, + sim_frame: SimulationFrame, + sim_local_constants, + agent_id, + sensor_state, + vehicle, + renderer, + bullet_client, + ) -> Tuple[Observation, bool, Dict[str, "Sensor"]]: + """Generate observations for the given agent around the given vehicle.""" + args = [sim_frame, sim_local_constants, agent_id, sensor_state, vehicle.id] + safe_obs, dones, updated_safe_sensors = cls.process_serialization_safe_sensors( + *args + ) + unsafe_obs, updated_unsafe_sensors = cls.process_serialization_unsafe_sensors( + *args, renderer, bullet_client + ) + complete_obs = safe_obs._replace( + **unsafe_obs, + ) + return (complete_obs, dones, {**updated_safe_sensors, **updated_unsafe_sensors}) + + @classmethod + def _agents_alive_done_check( + cls, ego_agent_ids, agent_ids, agents_alive: Optional[AgentsAliveDoneCriteria] + ): + if agents_alive is None: + return False + + if ( + agents_alive.minimum_ego_agents_alive + and len(ego_agent_ids) < agents_alive.minimum_ego_agents_alive + ): + return True + if ( + agents_alive.minimum_total_agents_alive + and len(agent_ids) < agents_alive.minimum_total_agents_alive + ): + return True + if agents_alive.agent_lists_alive: + for agents_list_alive in agents_alive.agent_lists_alive: + assert isinstance( + agents_list_alive.agents_list, (List, Set, Tuple) + ), "Please specify a list of agent ids to watch" + assert isinstance( + agents_list_alive.minimum_agents_alive_in_list, int + ), "Please specify an int for minimum number of alive agents in the list" + assert ( + agents_list_alive.minimum_agents_alive_in_list >= 0 + ), "minimum_agents_alive_in_list should not be negative" + agents_alive_check = [ + 1 if id in agent_ids else 0 for id in agents_list_alive.agents_list + ] + if ( + agents_alive_check.count(1) + < agents_list_alive.minimum_agents_alive_in_list + ): + return True + + return False + + @classmethod + def _actors_alive_done_check( + cls, + vehicle_ids, + sensor_state, + actors_alive: Optional[ActorsAliveDoneCriteria], + ): + if actors_alive is None: + return False + + sensor_state: SensorState = sensor_state + + pattern = re.compile( + "|".join(rf"(?:{aoi})" for aoi in actors_alive.actors_of_interest) + ) + ## TODO optimization to get vehicles that were added and removed last step + ## TODO second optimization to check for already known vehicles + for vehicle_id in vehicle_ids: + # get vehicles by pattern + if pattern.match(vehicle_id): + sensor_state.seen_interest_actors = True + return False + if actors_alive.strict or sensor_state.seen_interest_actors: + # if agent requires the actor to exist immediately + # OR if previously seen relevant actors but no actors match anymore + return True + + ## if never seen a relevant actor + return False + + @classmethod + def _is_done_with_events( + cls, + sim_frame: SimulationFrame, + sim_local_constants, + agent_id, + vehicle_state: VehicleState, + sensor_state, + plan, + vehicle_sensors, + ): + vehicle_sensors = sim_frame.vehicle_sensors[vehicle_state.actor_id] + interface = sim_frame.agent_interfaces[agent_id] + done_criteria = interface.done_criteria + event_config = interface.event_configuration + + # TODO: the following calls nearest_lanes (expensive) 6 times + reached_goal = cls._agent_reached_goal( + sensor_state, plan, vehicle_state, vehicle_sensors.get("trip_meter_sensor") + ) + collided = sim_frame.vehicle_did_collide(vehicle_state.actor_id) + is_off_road = cls._vehicle_is_off_road( + sim_local_constants.road_map, vehicle_state + ) + is_on_shoulder = cls._vehicle_is_on_shoulder( + sim_local_constants.road_map, vehicle_state + ) + is_not_moving = cls._vehicle_is_not_moving( + sim_frame, + event_config.not_moving_time, + event_config.not_moving_distance, + vehicle_sensors.get("driven_path_sensor"), + ) + reached_max_episode_steps = sensor_state.reached_max_episode_steps + is_off_route, is_wrong_way = cls._vehicle_is_off_route_and_wrong_way( + sim_frame, sim_local_constants, vehicle_state, plan + ) + agents_alive_done = cls._agents_alive_done_check( + sim_frame.ego_ids, sim_frame.potential_agent_ids, done_criteria.agents_alive + ) + actors_alive_done = cls._actors_alive_done_check( + sim_frame.vehicle_ids, sensor_state, done_criteria.actors_alive + ) + + done = not sim_frame.resetting and ( + (is_off_road and done_criteria.off_road) + or reached_goal + or reached_max_episode_steps + or (is_on_shoulder and done_criteria.on_shoulder) + or (collided and done_criteria.collision) + or (is_not_moving and done_criteria.not_moving) + or (is_off_route and done_criteria.off_route) + or (is_wrong_way and done_criteria.wrong_way) + or agents_alive_done + or actors_alive_done + ) + + events = Events( + collisions=sim_frame.filtered_vehicle_collisions(vehicle_state.actor_id), + off_road=is_off_road, + reached_goal=reached_goal, + reached_max_episode_steps=reached_max_episode_steps, + off_route=is_off_route, + on_shoulder=is_on_shoulder, + wrong_way=is_wrong_way, + not_moving=is_not_moving, + agents_alive_done=agents_alive_done, + actors_alive_done=actors_alive_done, + ) + + return done, events + + @classmethod + def _agent_reached_goal( + cls, sensor_state, plan, vehicle_state: VehicleState, trip_meter_sensor + ): + if not trip_meter_sensor: + return False + distance_travelled = trip_meter_sensor() + mission = plan.mission + return mission.is_complete(vehicle_state, distance_travelled) + + @classmethod + def _vehicle_is_off_road(cls, road_map, vehicle_state: VehicleState): + return not road_map.road_with_point(vehicle_state.pose.point) + + @classmethod + def _vehicle_is_on_shoulder(cls, road_map, vehicle_state: VehicleState): + # XXX: this isn't technically right as this would also return True + # for vehicles that are completely off road. + for corner_coordinate in vehicle_state.bounding_box_points: + if not road_map.road_with_point(Point(*corner_coordinate)): + return True + return False + + @classmethod + def _vehicle_is_not_moving( + cls, sim, last_n_seconds_considered, min_distance_moved, driven_path_sensor + ): + # Flag if the vehicle has been immobile for the past 'last_n_seconds_considered' seconds + if sim.elapsed_sim_time < last_n_seconds_considered: + return False + + distance = sys.maxsize + if driven_path_sensor is not None: + distance = driven_path_sensor.distance_travelled( + sim.elapsed_sim_time, last_n_seconds=last_n_seconds_considered + ) + + # Due to controller instabilities there may be some movement even when a + # vehicle is "stopped". + return distance < min_distance_moved + + @classmethod + def _vehicle_is_off_route_and_wrong_way( + cls, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + vehicle_state: VehicleState, + plan, + ): + """Determines if the agent is on route and on the correct side of the road. + + Args: + sim_frame: An instance of the simulator. + sim_local_constants: The current frozen state of the simulation for last reset. + vehicle_state: The current state of the vehicle to check. + plan: The current plan for the vehicle. + + Returns: + A tuple (is_off_route, is_wrong_way) + is_off_route: + Actor's vehicle is not on its route or an oncoming traffic lane. + is_wrong_way: + Actor's vehicle is going against the lane travel direction. + """ + + route_roads = plan.route.roads + + vehicle_pos = vehicle_state.pose.point + vehicle_minimum_radius_bounds = ( + np.linalg.norm(vehicle_state.dimensions.as_lwh[:2]) * 0.5 + ) + # Check that center of vehicle is still close to route + radius = vehicle_minimum_radius_bounds + 5 + nearest_lane = sim_local_constants.road_map.nearest_lane( + vehicle_pos, radius=radius + ) + + # No road nearby, so we're not on route! + if not nearest_lane: + return (True, False) + + # Check whether vehicle is in wrong-way + is_wrong_way = cls._check_wrong_way_event(nearest_lane, vehicle_state) + + # Check whether vehicle has no-route or is on-route + if ( + not route_roads # Vehicle has no-route. E.g., endless mission with a random route + or nearest_lane.road in route_roads # Vehicle is on-route + or nearest_lane.in_junction + ): + return (False, is_wrong_way) + + veh_offset = nearest_lane.offset_along_lane(vehicle_pos) + + # so we're obviously not on the route, but we might have just gone + # over the center line into an oncoming lane... + for on_lane in nearest_lane.oncoming_lanes_at_offset(veh_offset): + if on_lane.road in route_roads: + return (False, is_wrong_way) + + # Check for case if there was an early merge into another incoming lane. This means the + # vehicle should still be following the lane direction to be valid as still on route. + if not is_wrong_way: + # See if the lane leads into the current route + for lane in nearest_lane.outgoing_lanes: + if lane.road in route_roads: + return (False, is_wrong_way) + # If outgoing lane is in a junction see if the junction lane leads into current route. + if lane.in_junction: + for out_lane in lane.outgoing_lanes: + if out_lane.road in route_roads: + return (False, is_wrong_way) + + # Vehicle is completely off-route + return (True, is_wrong_way) + + @staticmethod + def _vehicle_is_wrong_way(vehicle_state: VehicleState, closest_lane): + target_pose = closest_lane.center_pose_at_point(vehicle_state.pose.point) + # Check if the vehicle heading is oriented away from the lane heading. + return ( + np.fabs(vehicle_state.pose.heading.relative_to(target_pose.heading)) + > 0.5 * np.pi + ) + + @classmethod + def _check_wrong_way_event(cls, lane_to_check, vehicle_state): + # When the vehicle is in an intersection, turn off the `wrong way` check to avoid + # false positive `wrong way` events. + if lane_to_check.in_junction: + return False + return cls._vehicle_is_wrong_way(vehicle_state, lane_to_check) diff --git a/smarts/core/sensors/local_sensor_resolver.py b/smarts/core/sensors/local_sensor_resolver.py new file mode 100644 index 0000000000..61b84968cd --- /dev/null +++ b/smarts/core/sensors/local_sensor_resolver.py @@ -0,0 +1,85 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging +from typing import Set + +from smarts.core.sensors import SensorResolver, Sensors +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.utils.file import replace +from smarts.core.utils.logging import timeit + +logger = logging.getLogger(__name__) + + +class LocalSensorResolver(SensorResolver): + """This implementation of the sensor resolver completes observations serially.""" + + def observe( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], + renderer, + bullet_client, + ): + with timeit("serial run", logger.info): + ( + observations, + dones, + updated_sensors, + ) = Sensors.observe_serializable_sensor_batch( + sim_frame, + sim_local_constants, + agent_ids, + ) + + # While observation processes are operating do rendering + with timeit("rendering", logger.info): + rendering = {} + for agent_id in agent_ids: + for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: + ( + rendering[agent_id], + updated_unsafe_sensors, + ) = Sensors.process_serialization_unsafe_sensors( + sim_frame, + sim_local_constants, + agent_id, + sim_frame.sensor_states[vehicle_id], + vehicle_id, + renderer, + bullet_client, + ) + updated_sensors[vehicle_id].update(updated_unsafe_sensors) + + with timeit(f"merging observations", logger.info): + # Merge sensor information + for agent_id, r_obs in rendering.items(): + observations[agent_id] = replace(observations[agent_id], **r_obs) + + return observations, dones, updated_sensors + + def step(self, sim_frame, sensor_states): + """Step the sensor state.""" + for sensor_state in sensor_states: + sensor_state.step() diff --git a/smarts/core/sensors/parallel_sensor_resolver.py b/smarts/core/sensors/parallel_sensor_resolver.py new file mode 100644 index 0000000000..1635118e99 --- /dev/null +++ b/smarts/core/sensors/parallel_sensor_resolver.py @@ -0,0 +1,390 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging +import multiprocessing as mp +from collections import defaultdict +from dataclasses import dataclass +from enum import IntEnum +from typing import Any, Dict, List, Optional, Set + +import psutil + +import smarts.core.serialization.default as serializer +from smarts.core import config +from smarts.core.sensors import SensorResolver, Sensors +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.utils.file import replace +from smarts.core.utils.logging import timeit + +logger = logging.getLogger(__name__) + + +class ParallelSensorResolver(SensorResolver): + """This implementation of the sensor resolver completes observations in parallel.""" + + def __init__(self, process_count_override: Optional[int] = None) -> None: + super().__init__() + self._logger = logging.getLogger("Sensors") + self._sim_local_constants: SimulationLocalConstants = None + self._workers: List[SensorsWorker] = [] + self._process_count_override = process_count_override + + def observe( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], + renderer, + bullet_client, + ): + """Runs observations in parallel where possible. + Args: + sim_frame (SimulationFrame): + The current state from the simulation. + sim_local_constants (SimulationLocalConstants): + The values that should stay the same for a simulation over a reset. + agent_ids ({str, ...}): + The agent ids to process. + renderer (Optional[Renderer]): + The renderer (if any) that should be used. + bullet_client (bc.BulletClient): + The physics client. + """ + observations, dones, updated_sensors = {}, {}, defaultdict(dict) + + num_spare_cpus = max(0, psutil.cpu_count(logical=False) - 1) + used_processes = ( + min( + config()("core", "observation_workers", default=8, cast=int), + num_spare_cpus, + ) + if self._process_count_override == None + else max(1, self._process_count_override) + ) + + workers: List[SensorsWorker] = self.get_workers( + used_processes, sim_local_constants=sim_local_constants + ) + used_workers: List[SensorsWorker] = [] + with timeit( + f"parallizable observations with {len(agent_ids)} and {len(workers)}", + logger.info, + ): + agent_ids_for_grouping = list(agent_ids) + agent_groups = [ + agent_ids_for_grouping[i::used_processes] for i in range(used_processes) + ] + worker_args = WorkerKwargs(sim_frame=sim_frame) + for i, agent_group in enumerate(agent_groups): + if not agent_group: + break + with timeit(f"submitting {len(agent_group)} agents", logger.info): + workers[i].send( + SensorsWorker.Request( + SensorsWorkerRequestId.SIMULATION_FRAME, + worker_args.merged(WorkerKwargs(agent_ids=agent_group)), + ) + ) + used_workers.append(workers[i]) + + # While observation processes are operating do rendering + with timeit("rendering", logger.info): + rendering = {} + for agent_id in agent_ids: + for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: + ( + rendering[agent_id], + updated_unsafe_sensors, + ) = Sensors.process_serialization_unsafe_sensors( + sim_frame, + sim_local_constants, + agent_id, + sim_frame.sensor_states[vehicle_id], + vehicle_id, + renderer, + bullet_client, + ) + updated_sensors[vehicle_id].update(updated_unsafe_sensors) + + # Collect futures + with timeit("waiting for observations", logger.info): + if used_workers: + while agent_ids != set(observations): + assert all( + w.running for w in used_workers + ), "A process worker crashed." + for result in mp.connection.wait( + [worker.connection for worker in used_workers], timeout=5 + ): + # pytype: disable=attribute-error + obs, ds, u_sens = result.recv() + # pytype: enable=attribute-error + observations.update(obs) + dones.update(ds) + for v_id, values in u_sens.items(): + updated_sensors[v_id].update(values) + + with timeit(f"merging observations", logger.info): + # Merge sensor information + for agent_id, r_obs in rendering.items(): + observations[agent_id] = replace(observations[agent_id], **r_obs) + + return observations, dones, updated_sensors + + def __del__(self): + try: + self.stop_all_workers() + except AttributeError: + pass + + def stop_all_workers(self): + """Stop all current workers and clear reference to them.""" + for worker in self._workers: + worker.stop() + self._workers = [] + + def _validate_configuration(self, local_constants: SimulationLocalConstants): + """Check that constants have not changed which might indicate that the workers need to be updated.""" + return local_constants == self._sim_local_constants + + def generate_workers( + self, count, workers_list: List[Any], worker_kwargs: "WorkerKwargs" + ): + """Generate the given number of workers requested.""" + while len(workers_list) < count: + new_worker = SensorsWorker() + workers_list.append(new_worker) + new_worker.run() + new_worker.send( + request=SensorsWorker.Request( + SensorsWorkerRequestId.SIMULATION_LOCAL_CONSTANTS, worker_kwargs + ) + ) + + def get_workers( + self, count, sim_local_constants: SimulationLocalConstants, **kwargs + ) -> List["SensorsWorker"]: + """Get the give number of workers.""" + if not self._validate_configuration(sim_local_constants): + self.stop_all_workers() + self._sim_local_constants = sim_local_constants + if len(self._workers) < count: + worker_kwargs = WorkerKwargs( + **kwargs, sim_local_constants=sim_local_constants + ) + self.generate_workers(count, self._workers, worker_kwargs) + return self._workers[:count] + + def step(self, sim_frame, sensor_states): + """Step the sensor state.""" + for sensor_state in sensor_states: + sensor_state.step() + + @property + def process_count_override(self) -> Optional[int]: + """The number of processes this implementation should run. + + Returns: + int: Number of processes. + """ + return self._process_count_override + + @process_count_override.setter + def process_count_override(self, count: Optional[int]): + self._process_count_override = count + + +class WorkerKwargs: + """Used to serialize arguments for a worker upfront.""" + + def __init__(self, **kwargs) -> None: + self.kwargs = self._serialize(kwargs) + + def merged(self, o_worker_kwargs: "WorkerKwargs") -> "WorkerKwargs": + """Merge two worker arguments and return a new copy.""" + new = type(self)() + new.kwargs = {**self.kwargs, **o_worker_kwargs.kwargs} + return new + + @staticmethod + def _serialize(kwargs: Dict): + return { + k: serializer.dumps(a) if a is not None else a for k, a in kwargs.items() + } + + def deserialize(self): + """Deserialize all objects in the arguments and return a dictionary copy.""" + return { + k: serializer.loads(a) if a is not None else a + for k, a in self.kwargs.items() + } + + +class ProcessWorker: + """A utility class that defines a persistant worker which will continue to operate in the background.""" + + class WorkerDone: + """The done signal for a worker.""" + + pass + + @dataclass + class Request: + """A request to made to the process worker""" + + id: Any + data: WorkerKwargs + + def __init__(self, serialize_results=False) -> None: + parent_connection, child_connection = mp.Pipe() + self._parent_connection = parent_connection + self._child_connection = child_connection + self._serialize_results = serialize_results + self._proc: Optional[mp.Process] = None + + @classmethod + def _do_work(cls, state): + raise NotImplementedError() + + @classmethod + def _on_request(cls, state: Dict, request: Request) -> bool: + """ + Args: + state: The persistant state on the worker + request: A request made to the worker. + + Returns: + bool: If the worker method `_do_work` should be called. + """ + raise NotImplementedError() + + @classmethod + def _run( + cls: "ProcessWorker", + connection: mp.connection.Connection, + serialize_results, + ): + state: Dict[Any, Any] = {} + while True: + run_work = False + work = connection.recv() + if isinstance(work, cls.WorkerDone): + break + if isinstance(work, cls.Request): + run_work = cls._on_request(state, request=work) + with timeit("do work", logger.info): + if not run_work: + continue + result = cls._do_work(state=state.copy()) + with timeit("reserialize", logger.info): + if serialize_results: + result = serializer.dumps(result) + with timeit("put back to main thread", logger.info): + connection.send(result) + + def run(self): + """Start the worker seeded with the given data.""" + kwargs = dict(serialize_results=self._serialize_results) + self._proc = mp.Process( + target=self._run, + args=(self._child_connection,), + kwargs=kwargs, + daemon=True, + ) + self._proc.start() + return self._parent_connection + + def send(self, request: Request): + """Sends a request to the worker.""" + assert isinstance(request, self.Request) + self._parent_connection.send(request) + + def result(self, timeout=None): + """The most recent result from the worker.""" + with timeit("main thread blocked", logger.info): + conn = mp.connection.wait([self._parent_connection], timeout=timeout).pop() + # pytype: disable=attribute-error + result = conn.recv() + # pytype: enable=attribute-error + with timeit("deserialize for main thread", logger.info): + if self._serialize_results: + result = serializer.loads(result) + return result + + def stop(self): + """Sends a stop signal to the worker.""" + try: + self._parent_connection.send(self.WorkerDone()) + except ImportError: + # Python is shutting down. + if not self._parent_connection.closed: + self._parent_connection.close() + + @property + def running(self) -> bool: + """If this current worker is still running.""" + return self._proc is not None and self._proc.exitcode is None + + @property + def connection(self): + """The underlying connection to send data to the worker.""" + return self._parent_connection + + +class SensorsWorkerRequestId(IntEnum): + """Options for update requests to a sensor worker.""" + + SIMULATION_FRAME = 1 + SIMULATION_LOCAL_CONSTANTS = 2 + + +class SensorsWorker(ProcessWorker): + """A worker for sensors.""" + + def __init__(self) -> None: + super().__init__() + + @classmethod + def _do_work(cls, state): + return cls.local(state=state) + + @classmethod + def _on_request(cls, state: Dict, request: ProcessWorker.Request) -> bool: + assert request.data is None or isinstance(request.data, WorkerKwargs) + if request.id == SensorsWorkerRequestId.SIMULATION_FRAME: + state.update(request.data.deserialize()) + return True + if request.id == SensorsWorkerRequestId.SIMULATION_LOCAL_CONSTANTS: + state.update(request.data.deserialize()) + + return False + + @staticmethod + def local(state: Dict): + """The work method on the local thread.""" + sim_local_constants = state["sim_local_constants"] + sim_frame = state["sim_frame"] + agent_ids = state["agent_ids"] + return Sensors.observe_serializable_sensor_batch( + sim_frame, sim_local_constants, agent_ids + ) diff --git a/smarts/core/serialization/default.py b/smarts/core/serialization/default.py new file mode 100644 index 0000000000..58e732f245 --- /dev/null +++ b/smarts/core/serialization/default.py @@ -0,0 +1,88 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from dataclasses import dataclass +from typing import Any + +_proxies = {} + + +def dumps(__o): + """Serializes the given object.""" + import cloudpickle + + _lazy_init() + r = __o + type_ = type(__o) + # TODO: Add a formatter parameter instead of handling proxies internal to serialization + proxy_func = _proxies.get(type_) + if proxy_func: + r = proxy_func(__o) + return cloudpickle.dumps(r) + + +def loads(__o): + """Deserializes the given object.""" + import cloudpickle + + r = cloudpickle.loads(__o) + if hasattr(r, "deproxy"): + r = r.deproxy() + return r + + +class Proxy: + """Defines a proxy object used to facilitate serialization of a non-serializable object.""" + + def deproxy(self): + """Convert the proxy back into the original object.""" + raise NotImplementedError() + + +@dataclass(frozen=True) +class _SimulationLocalConstantsProxy(Proxy): + road_map_spec: Any + road_map_hash: int + + def __eq__(self, __o: object) -> bool: + if __o is None: + return False + return self.road_map_hash == getattr(__o, "road_map_hash") + + def deproxy(self): + import smarts.sstudio.types + from smarts.core.simulation_local_constants import SimulationLocalConstants + + assert isinstance(self.road_map_spec, smarts.sstudio.types.MapSpec) + road_map, _ = self.road_map_spec.builder_fn(self.road_map_spec) + return SimulationLocalConstants(road_map, self.road_map_hash) + + +def _proxy_slc(v): + return _SimulationLocalConstantsProxy(v.road_map.map_spec, v.road_map_hash) + + +def _lazy_init(): + if len(_proxies) > 0: + return + from smarts.core.simulation_local_constants import SimulationLocalConstants + + _proxies[SimulationLocalConstants] = _proxy_slc diff --git a/smarts/core/signal_provider.py b/smarts/core/signal_provider.py index ca1494a31c..df67b582bd 100644 --- a/smarts/core/signal_provider.py +++ b/smarts/core/signal_provider.py @@ -21,7 +21,7 @@ from .actor import ActorRole, ActorState from .controllers import ActionSpaceType -from .provider import Provider, ProviderRecoveryFlags, ProviderState +from .provider import Provider, ProviderManager, ProviderRecoveryFlags, ProviderState from .road_map import RoadMap from .scenario import Scenario from .signals import SignalLightState, SignalState @@ -50,6 +50,9 @@ def actions(self) -> Set[ActionSpaceType]: # (Then the action space could just be the desired SignalLightState.) return set() + def set_manager(self, manager: ProviderManager): + pass + @property def _provider_state(self) -> ProviderState: return ProviderState(actors=list(self._my_signals.values())) @@ -62,7 +65,8 @@ def setup(self, scenario: Scenario) -> ProviderState: if scenario.traffic_history is None and not scenario.supports_sumo_traffic: for feature in self._road_map.dynamic_features: if feature.type == RoadMap.FeatureType.FIXED_LOC_SIGNAL: - controlled_lanes = [feature.type_specific_info] + feature_lane = feature.type_specific_info + controlled_lanes = [feature_lane.lane_id] self._my_signals[feature.feature_id] = SignalState( actor_id=feature.feature_id, actor_type="signal", diff --git a/smarts/core/signals.py b/smarts/core/signals.py index ffed31c1b2..e4e5354d6a 100644 --- a/smarts/core/signals.py +++ b/smarts/core/signals.py @@ -21,7 +21,7 @@ from enum import IntFlag from typing import List, Optional -from .actor import ActorRole, ActorState +from .actor import ActorState from .coordinates import Point from .road_map import RoadMap @@ -45,7 +45,7 @@ class SignalState(ActorState): state: Optional[SignalLightState] = None stopping_pos: Optional[Point] = None - controlled_lanes: Optional[List[RoadMap.Lane]] = None + controlled_lanes: Optional[List[str]] = None last_changed: Optional[float] = None # will be None if not known def __post_init__(self): diff --git a/smarts/core/simulation_frame.py b/smarts/core/simulation_frame.py new file mode 100644 index 0000000000..c9251a0f02 --- /dev/null +++ b/smarts/core/simulation_frame.py @@ -0,0 +1,99 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import logging +from dataclasses import dataclass +from typing import Any, Dict, List, Optional, Set + +from cached_property import cached_property + +from smarts.core.actor import ActorState +from smarts.core.agent_interface import AgentInterface +from smarts.core.vehicle_state import Collision, VehicleState + +logger = logging.getLogger(__name__) + + +@dataclass(frozen=True) +class SimulationFrame: + """This is state that should change per step of the simulation.""" + + actor_states: List[ActorState] + agent_vehicle_controls: Dict[str, str] + agent_interfaces: Dict[str, AgentInterface] + ego_ids: str + pending_agent_ids: List[str] + elapsed_sim_time: float + fixed_timestep: float + resetting: bool + map_spec: Any + last_dt: float + last_provider_state: Any + step_count: int + vehicle_collisions: Dict[str, List[Collision]] + vehicles_for_agents: Dict[str, List[str]] + vehicle_ids: Set[str] + vehicle_states: Dict[str, VehicleState] + vehicle_sensors: Dict[str, Dict[str, Any]] + + sensor_states: Any + # TODO MTA: renderer can be allowed here as long as it is only type information + # renderer_type: Any = None + _ground_bullet_id: Optional[str] = None + + @cached_property + def agent_ids(self) -> Set[str]: + """Get the ids of all agents that currently have vehicles.""" + return set(self.vehicles_for_agents.keys()) + + @cached_property + def potential_agent_ids(self) -> Set[str]: + """This includes current agent ids and future pending ego agent ids.""" + return set(self.vehicles_for_agents.keys()) | set(self.pending_agent_ids) + + @cached_property + def actor_states_by_id(self) -> Dict[str, ActorState]: + """Get actor states paired by their ids.""" + return {a_s.actor_id: a_s for a_s in self.actor_states} + + def vehicle_did_collide(self, vehicle_id) -> bool: + """Test if the given vehicle had any collisions in the last physics update.""" + vehicle_collisions = self.vehicle_collisions.get(vehicle_id, []) + for c in vehicle_collisions: + if c.collidee_id != self._ground_bullet_id: + return True + return False + + def filtered_vehicle_collisions(self, vehicle_id) -> List[Collision]: + """Get a list of all collisions the given vehicle was involved in during the last + physics update. + """ + vehicle_collisions = self.vehicle_collisions.get(vehicle_id, []) + return [ + c for c in vehicle_collisions if c.collidee_id != self._ground_bullet_id + ] + + def __post_init__(self): + if logger.isEnabledFor(logging.DEBUG): + assert isinstance(self.actor_states, list) + assert self.agent_ids.union(self.vehicles_for_agents) == self.agent_ids + assert len(self.agent_ids - set(self.agent_interfaces)) == 0 + assert not len(self.vehicle_ids.symmetric_difference(self.vehicle_states)) diff --git a/smarts/core/simulation_local_constants.py b/smarts/core/simulation_local_constants.py new file mode 100644 index 0000000000..b65d056e9c --- /dev/null +++ b/smarts/core/simulation_local_constants.py @@ -0,0 +1,38 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from dataclasses import dataclass +from typing import Any + + +# TODO MTA: Consider using EzPickle base +@dataclass(frozen=True) +class SimulationLocalConstants: + """This is state that should only change every reset.""" + + road_map: Any + road_map_hash: int + + def __eq__(self, __o: object) -> bool: + if __o is None: + return False + assert isinstance(__o, SimulationLocalConstants) + return self.road_map_hash == __o.road_map_hash diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index 8cf7c23f0e..f6583426e0 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -24,7 +24,7 @@ from typing import Any, Dict, Iterable, List, Optional, Sequence, Set, Tuple, Union import numpy as np -from scipy.spatial.distance import cdist +from cached_property import cached_property from envision import types as envision_types from envision.client import Client as EnvisionClient @@ -32,6 +32,7 @@ from smarts.core.actor_capture_manager import ActorCaptureManager from smarts.core.id_actor_capture_manager import IdActorCaptureManager from smarts.core.plan import Plan +from smarts.core.simulation_local_constants import SimulationLocalConstants from smarts.core.utils.logging import timeit from . import config, models @@ -49,12 +50,14 @@ from .controllers import ActionSpaceType from .coordinates import BoundingBox, Point from .external_provider import ExternalProvider -from .observations import Collision, Observation +from .observations import Observation from .provider import Provider, ProviderManager, ProviderRecoveryFlags, ProviderState from .road_map import RoadMap from .scenario import Mission, Scenario +from .sensor_manager import SensorManager from .signal_provider import SignalProvider from .signals import SignalLightState, SignalState +from .simulation_frame import SimulationFrame from .traffic_history_provider import TrafficHistoryProvider from .traffic_provider import TrafficProvider from .trap_manager import TrapManager @@ -63,8 +66,9 @@ from .utils.math import rounder_for_dt from .utils.pybullet import bullet_client as bc from .utils.visdom_client import VisdomClient -from .vehicle import Vehicle, VehicleState +from .vehicle import Vehicle from .vehicle_index import VehicleIndex +from .vehicle_state import Collision, VehicleState, neighborhood_vehicles_around_vehicle logging.basicConfig( format="%(asctime)s.%(msecs)03d %(levelname)s: {%(module)s} %(message)s", @@ -184,6 +188,7 @@ def __init__( ) # Set up indices + self._sensor_manager = SensorManager() self._vehicle_index = VehicleIndex() self._agent_manager = AgentManager(self, agent_interfaces, zoo_addrs) @@ -212,12 +217,14 @@ def step( Dict[str, Dict[str, float]], ]: """Progress the simulation by a fixed or specified time. - - :param agent_actions: Actions that the agents want to perform on their actors. - :param time_delta_since_last_step: Overrides the simulation step length. - Progress simulation time by the given amount. - Note the time_delta_since_last_step param is in (nominal) seconds. - :return: observations, rewards, dones, infos + Args: + agent_actions: + Actions that the agents want to perform on their actors. + time_delta_since_last_step: + Overrides the simulation step length. Progress simulation time by the given amount. + Note the time_delta_since_last_step param is in (nominal) seconds. + Returns: + observations, rewards, dones, infos """ if not self._is_setup: raise SMARTSNotSetupError("Must call reset() or setup() before stepping.") @@ -232,14 +239,16 @@ def step( except (KeyboardInterrupt, SystemExit): # ensure we clean-up if the user exits the simulation self._log.info("Simulation was interrupted by the user.") - self.destroy() + if not config()("core", "debug", default=False, cast=bool): + self.destroy() raise # re-raise the KeyboardInterrupt except Exception as e: self._log.error( "Simulation crashed with exception. Attempting to cleanly shutdown." ) self._log.exception(e) - self.destroy() + if not config().get_setting("core", "debug", default=False, cast=bool): + self.destroy() raise # re-raise def _check_if_acting_on_active_agents(self, agent_actions): @@ -303,17 +312,29 @@ def _step(self, agent_actions, time_delta_since_last_step: Optional[float] = Non # This is a hack to give us some short term perf wins. Longer term we # need to expose better support for batched computations self._vehicle_states = [v.state for v in self._vehicle_index.vehicles] + self._sensor_manager.clean_up_sensors_for_actors( + set(v.actor_id for v in self._vehicle_states), renderer=self.renderer_ref + ) + + # Reset frame state + try: + del self.cached_frame + except AttributeError: + pass + self.cached_frame # Agents with timeit("Stepping through sensors", self._log.debug): - self._agent_manager.step_sensors() + self._sensor_manager.step(self.cached_frame, self.renderer_ref) - if self._renderer: + if self.renderer_ref: # runs through the render pipeline (for camera-based sensors) - # MUST perform this after step_sensors() above, and before observe() below, + # MUST perform this after _sensor_manager.step() above, and before observe() below, # so that all updates are ready before rendering happens per + with timeit("Syncing the renderer", self._log.debug): + self.renderer_ref.sync(self.cached_frame) with timeit("Running through the render pipeline", self._log.debug): - self._renderer.render() + self.renderer_ref.render() with timeit("Calculating observations and rewards", self._log.debug): observations, rewards, scores, dones = self._agent_manager.observe() @@ -351,7 +372,8 @@ def done_vehicle_ids(dones): if self._agent_manager.is_boid_agent(agent_id): vehicle_ids.update(id_ for id_ in done if done[id_]) elif done: - ids = self._vehicle_index.vehicle_ids_by_actor_id(agent_id) + ids = self._vehicle_index.vehicle_ids_by_owner_id(agent_id) + # 0 if shadowing, 1 if active assert len(ids) <= 1, f"{len(ids)} <= 1" vehicle_ids.update(ids) @@ -383,33 +405,33 @@ def reset( self, scenario: Scenario, start_time: float = 0.0 ) -> Dict[str, Observation]: """Reset the simulation, reinitialize with the specified scenario. Then progress the - simulation up to the first time an agent returns an observation, or ``start_time`` if there + simulation up to the first time an agent returns an observation, or `start_time` if there are no agents in the simulation. - - :param scenario: The scenario to reset the simulation with. - :type scenario: class: Scenario - :param start_time: + Args: + scenario(Scenario): + The scenario to reset the simulation with. + start_time(float): The initial amount of simulation time to skip. This has implications on all time dependent systems. NOTE: SMARTS simulates a step and then updates vehicle control. - If you want a vehicle to enter at exactly ``0.3`` with a step of ``0.1`` it means the - simulation should start at ``start_time==0.2``. - :type start_time: float - :return: Agent observations. This observation is as follows: - - If no agents: the initial simulation observation at ``start_time`` - - If agents: the first step of the simulation with an agent observation + If you want a vehicle to enter at exactly `0.3` with a step of `0.1` it means the + simulation should start at `start_time==0.2`. + Returns: + Agent observations. This observation is as follows: + - If no agents: the initial simulation observation at `start_time` + - If agents: the first step of the simulation with an agent observation """ - tries = 2 + tries = config()("core", "reset_retries", 0, cast=int) + 1 first_exception = None for _ in range(tries): try: self._resetting = True return self._reset(scenario, start_time) - except Exception as e: + except Exception as err: if not first_exception: - first_exception = e + first_exception = err finally: self._resetting = False - self._log.error(f"Failed to successfully reset after {tries} times.") + self._log.error("Failed to successfully reset after %i tries.", tries) raise first_exception def _reset(self, scenario: Scenario, start_time: float): @@ -428,7 +450,7 @@ def _reset(self, scenario: Scenario, start_time: float): agent_ids = self._agent_manager.teardown_ego_agents() agent_ids |= self.agent_manager.teardown_social_agents() for agent_id in agent_ids: - ids = self._vehicle_index.vehicle_ids_by_actor_id(agent_id) + ids = self._vehicle_index.vehicle_ids_by_owner_id(agent_id) vehicle_ids_to_teardown |= set(ids) self._teardown_vehicles(set(vehicle_ids_to_teardown)) self._reset_providers() @@ -436,12 +458,16 @@ def _reset(self, scenario: Scenario, start_time: float): actor_capture_manager.reset(scenario, self) self._agent_manager.init_ego_agents() if self._renderer: - self._sync_vehicles_to_renderer() + self._renderer.reset() else: self.teardown() self._reset_providers() self.setup(scenario) + if not self.local_constants.road_map.is_same_map(scenario.map_spec): + del self.local_constants + self.local_constants + # Tell history provide to ignore vehicles if we have assigned mission to them self._traffic_history_provider.set_replaced_ids( m.vehicle_spec.veh_id @@ -451,6 +477,10 @@ def _reset(self, scenario: Scenario, start_time: float): self._reset_required = False self._vehicle_states = [v.state for v in self._vehicle_index.vehicles] + try: + del self.cached_frame + except AttributeError: + pass observations, _, _, _ = self._agent_manager.observe() observations_for_ego = self._agent_manager.reset_agents(observations) @@ -674,13 +704,11 @@ def vehicle_exited_bubble( original_agent_id = agent_id agent_id = None # FIXME: This only gets the first shadow agent and this shadow agent is not specific to a bubble!!!!!! - shadow_agent_id = self._vehicle_index.shadow_actor_id_from_vehicle_id( - vehicle_id - ) + shadow_agent_id = self._vehicle_index.shadower_id_from_vehicle_id(vehicle_id) if shadow_agent_id is not None: assert original_agent_id == shadow_agent_id if self._vehicle_index.vehicle_is_hijacked(vehicle_id): - agent_id = self._vehicle_index.actor_id_from_vehicle_id(vehicle_id) + agent_id = self._vehicle_index.owner_id_from_vehicle_id(vehicle_id) assert agent_id == original_agent_id self._log.debug( "agent=%s relinquishing vehicle=%s (shadow_agent=%s)", @@ -689,7 +717,7 @@ def vehicle_exited_bubble( shadow_agent_id, ) state, route = self._vehicle_index.relinquish_agent_control( - self, vehicle_id + self, vehicle_id, self.road_map ) new_prov = self._agent_relinquishing_actor(agent_id, state, teardown_agent) if ( @@ -707,17 +735,8 @@ def vehicle_exited_bubble( self._vehicle_index.stop_shadowing(shadow_agent_id, vehicle_id) if teardown_agent: self.teardown_social_agents([shadow_agent_id]) - if self._vehicle_index.shadow_actor_id_from_vehicle_id(vehicle_id) is None: - self._agent_manager.detach_sensors_from_vehicle(vehicle_id) - - if teardown_agent: - active_agents = self._agent_manager.active_agents - assert ( - shadow_agent_id not in active_agents - ), f"Agent ids {shadow_agent_id}, {active_agents}" - assert ( - agent_id not in active_agents - ), f"Agent id `{agent_id}` not in {active_agents}`" + if self._vehicle_index.shadower_id_from_vehicle_id(vehicle_id) is None: + self._sensor_manager.remove_sensors_by_actor_id(vehicle_id) def _agent_relinquishing_actor( self, @@ -836,7 +855,9 @@ def teardown(self): if self._agent_manager is not None: self._agent_manager.teardown() if self._vehicle_index is not None: - self._vehicle_index.teardown() + self._vehicle_index.teardown(self.renderer_ref) + if self._sensor_manager is not None: + self._sensor_manager.teardown(self.renderer_ref) if self._bullet_client is not None: self._bullet_client.resetSimulation() @@ -906,7 +927,9 @@ def __del__(self): raise exception def _teardown_vehicles(self, vehicle_ids): - self._vehicle_index.teardown_vehicles_by_vehicle_ids(vehicle_ids) + self._vehicle_index.teardown_vehicles_by_vehicle_ids( + vehicle_ids, self.renderer_ref + ) self._clear_collisions(vehicle_ids) for v_id in vehicle_ids: self._remove_vehicle_from_providers(v_id) @@ -916,7 +939,11 @@ def attach_sensors_to_vehicles(self, agent_interface, vehicle_ids): interface. """ self._check_valid() - self._agent_manager.attach_sensors_to_vehicles(agent_interface, vehicle_ids) + for v_id in vehicle_ids: + v = self._vehicle_index.vehicle_by_id(v_id) + Vehicle.attach_sensors_to_vehicle( + self._sensor_manager, self, v, agent_interface + ) def observe_from( self, vehicle_ids: Sequence[str] @@ -950,6 +977,11 @@ def renderer(self): self._vehicle_index.begin_rendering_vehicles(self._renderer) return self._renderer + @property + def renderer_ref(self) -> Optional[Any]: + """Get the reference of the renderer. This can be `None`.""" + return self._renderer + @property def is_rendering(self) -> bool: """If the simulation has image rendering active.""" @@ -1022,23 +1054,24 @@ def version(self) -> str: def teardown_social_agents(self, agent_ids: Iterable[str]): """ - Teardown agents in the given sequence. - - :param agent_ids: Sequence of agent ids + Teardown agents in the given sequence + Params: + agent_ids: Sequence of agent ids """ agents_to_teardown = { id_ for id_ in agent_ids - if not self.agent_manager.is_boid_keep_alive_agent(id_) + if not self.agent_manager.is_boid_agent(id_) + or self.agent_manager.is_boid_done(id_) } self.agent_manager.teardown_social_agents(filter_ids=agents_to_teardown) def teardown_social_agents_without_actors(self, agent_ids: Iterable[str]): """ Teardown agents in the given list that have no actors registered as - controlled-by or shadowed-by - - :param agent_ids: Sequence of agent ids + controlled-by or shadowed-by (for each given agent.) + Params: + agent_ids: Sequence of agent ids """ self._check_valid() original_agents = set(agent_ids) @@ -1047,7 +1080,7 @@ def teardown_social_agents_without_actors(self, agent_ids: Iterable[str]): for agent_id in original_agents # Only clean-up when there is no actor association left if len( - self._vehicle_index.vehicles_by_actor_id( + self._vehicle_index.vehicles_by_owner_id( agent_id, include_shadowers=True ) ) @@ -1066,17 +1099,19 @@ def teardown_social_agents_without_actors(self, agent_ids: Iterable[str]): def _teardown_vehicles_and_agents(self, vehicle_ids): shadow_and_controlling_agents = set() for vehicle_id in vehicle_ids: - agent_id = self._vehicle_index.actor_id_from_vehicle_id(vehicle_id) + agent_id = self._vehicle_index.owner_id_from_vehicle_id(vehicle_id) if agent_id: shadow_and_controlling_agents.add(agent_id) - shadow_agent_id = self._vehicle_index.shadow_actor_id_from_vehicle_id( + shadow_agent_id = self._vehicle_index.shadower_id_from_vehicle_id( vehicle_id ) if shadow_agent_id: shadow_and_controlling_agents.add(shadow_agent_id) - self._vehicle_index.teardown_vehicles_by_vehicle_ids(vehicle_ids) + self._vehicle_index.teardown_vehicles_by_vehicle_ids( + vehicle_ids, self.renderer_ref + ) self.teardown_social_agents_without_actors(shadow_and_controlling_agents) # XXX: don't remove vehicle from its (traffic) Provider here, as it may be being teleported # (and needs to remain registered in Traci during this step). @@ -1106,7 +1141,7 @@ def _pybullet_provider_sync(self, provider_state: ProviderState): social_vehicle = self._vehicle_index.build_social_vehicle( sim=self, vehicle_state=vehicle, - actor_id=vehicle_id, + owner_id="", vehicle_id=vehicle_id, ) @@ -1125,7 +1160,7 @@ def _step_pybullet(self): vehicle.step(self._elapsed_sim_time) @property - def vehicle_index(self): + def vehicle_index(self) -> VehicleIndex: """The vehicle index for direct vehicle manipulation.""" return self._vehicle_index @@ -1134,6 +1169,11 @@ def agent_manager(self) -> AgentManager: """The agent manager for direct agent manipulation.""" return self._agent_manager + @property + def sensor_manager(self) -> SensorManager: + """The sensor manager for direct manipulation.""" + return self._sensor_manager + @property def providers(self) -> List[Provider]: """The current providers controlling actors within the simulation.""" @@ -1169,8 +1209,6 @@ def _harmonize_providers(self, provider_state: ProviderState): except Exception as provider_error: self._handle_provider(provider, provider_error) self._pybullet_provider_sync(provider_state) - if self._renderer: - self._sync_vehicles_to_renderer() def _reset_providers(self): for provider in self.providers: @@ -1227,7 +1265,7 @@ def _provider_actions( vehicle_actions = dict() for agent_id, action in actions.items(): # TAI: reconsider include_shadowers = True - vehicles = self._vehicle_index.vehicles_by_actor_id( + vehicles = self._vehicle_index.vehicles_by_owner_id( agent_id, include_shadowers=True ) if not vehicles: @@ -1339,54 +1377,21 @@ def last_dt(self) -> float: return self._last_dt def neighborhood_vehicles_around_vehicle( - self, vehicle: Vehicle, radius: Optional[float] = None + self, vehicle_id: str, radius: Optional[float] = None ) -> List[VehicleState]: """Find vehicles in the vicinity of the target vehicle.""" self._check_valid() - other_states = [v for v in self._vehicle_states if v.actor_id != vehicle.id] - if radius is None: - return other_states - - other_positions = [state.pose.position for state in other_states] - if not other_positions: - return [] - - # calculate euclidean distances - distances = cdist( - other_positions, [vehicle.position], metric="euclidean" - ).reshape(-1) + from smarts.core.sensors import Sensors - indices = np.argwhere(distances <= radius).flatten() - return [other_states[i] for i in indices] - - def vehicle_did_collide(self, vehicle_id) -> bool: - """Test if the given vehicle had any collisions in the last physics update.""" - self._check_valid() - vehicle_collisions = self._vehicle_collisions.get(vehicle_id, []) - for c in vehicle_collisions: - if c.collidee_id != self._ground_bullet_id: - return True - return False - - def vehicle_collisions(self, vehicle_id) -> List[Collision]: - """Get a list of all collisions the given vehicle was involved in during the last - physics update. - """ - self._check_valid() - vehicle_collisions = self._vehicle_collisions.get(vehicle_id, []) - return [ - c for c in vehicle_collisions if c.collidee_id != self._ground_bullet_id - ] + vehicle = self._vehicle_index.vehicle_by_id(vehicle_id) + return neighborhood_vehicles_around_vehicle( + vehicle.state, self._vehicle_states, radius + ) def _clear_collisions(self, vehicle_ids): for vehicle_id in vehicle_ids: self._vehicle_collisions.pop(vehicle_id, None) - def _sync_vehicles_to_renderer(self): - assert self._renderer - for vehicle in self._vehicle_index.vehicles: - vehicle.sync_to_renderer() - def _get_pybullet_collisions(self, vehicle_id: str) -> Set[str]: vehicle = self._vehicle_index.vehicle_by_id(vehicle_id) # We are only concerned with vehicle-vehicle collisions @@ -1406,7 +1411,7 @@ def _process_collisions(self): vehicle_collisions = self._vehicle_collisions.setdefault(vehicle_id, []) for bullet_id in collidee_bullet_ids: collidee = self._bullet_id_to_vehicle(bullet_id) - actor_id = self._vehicle_index.actor_id_from_vehicle_id(collidee.id) + actor_id = self._vehicle_index.owner_id_from_vehicle_id(collidee.id) # TODO: Should we specify the collidee as the vehicle ID instead of # the agent/social ID? collision = Collision(collidee_id=actor_id) @@ -1500,7 +1505,7 @@ def _try_emit_envision_state(self, provider_state: ProviderState, obs, scores): continue if v.actor_id in agent_vehicle_ids: # this is an agent controlled vehicle - agent_id = self._vehicle_index.actor_id_from_vehicle_id(v.actor_id) + agent_id = self._vehicle_index.owner_id_from_vehicle_id(v.actor_id) is_boid_agent = self._agent_manager.is_boid_agent(agent_id) agent_obs = obs[agent_id] vehicle_obs = agent_obs[v.actor_id] if is_boid_agent else agent_obs @@ -1550,9 +1555,9 @@ def _try_emit_envision_state(self, provider_state: ProviderState, obs, scores): actor_type = envision_types.TrafficActorType.Agent if filter.actor_data_filter["mission_route_geometry"].enabled: mission_route_geometry = ( - self._vehicle_index.sensor_state_for_vehicle_id( - v.actor_id - ).plan.route.geometry + self._sensor_manager.sensor_state_for_actor_id(v.actor_id) + .get_plan(self.road_map) + .route.geometry ) else: actor_type = envision_types.TrafficActorType.SocialAgent @@ -1623,3 +1628,56 @@ def _try_emit_visdom_obs(self, obs): if not self._visdom: return self._visdom.send(obs) + + @cached_property + def local_constants(self): + """Generate the frozen state that should not change until next reset.""" + self._check_valid() + road_map, road_map_hash = self.scenario.map_spec.builder_fn( + self.scenario.map_spec + ) + return SimulationLocalConstants( + road_map=road_map, + road_map_hash=road_map_hash, + ) + + @cached_property + def cached_frame(self): + """Generate a frozen frame state of the simulation.""" + self._check_valid() + agent_actor_ids = self.vehicle_index.agent_vehicle_ids() + actor_states = self._last_provider_state + vehicles = dict(self.vehicle_index.vehicleitems()) + vehicle_ids = set(vid for vid in vehicles) + return SimulationFrame( + actor_states=getattr(actor_states, "actors", {}), + agent_interfaces=self.agent_manager.agent_interfaces.copy(), + agent_vehicle_controls={ + a_id: self.vehicle_index.owner_id_from_vehicle_id(a_id) + for a_id in agent_actor_ids + }, + ego_ids=self.agent_manager.ego_agent_ids, + pending_agent_ids=self.agent_manager.pending_agent_ids, + elapsed_sim_time=self.elapsed_sim_time, + fixed_timestep=self.fixed_timestep_sec, + resetting=self.resetting, + # road_map = self.road_map, + map_spec=self.scenario.map_spec, + last_dt=self.last_dt, + last_provider_state=self._last_provider_state, + step_count=self.step_count, + vehicle_collisions=self._vehicle_collisions, + vehicle_states={ + vehicle_id: vehicle.state for vehicle_id, vehicle in vehicles.items() + }, + vehicles_for_agents={ + agent_id: self.vehicle_index.vehicle_ids_by_owner_id( + agent_id, include_shadowers=True + ) + for agent_id in self.agent_manager.active_agents + }, + vehicle_ids=vehicle_ids, + vehicle_sensors=self.sensor_manager.sensors_for_actor_ids(vehicle_ids), + sensor_states=dict(self.sensor_manager.sensor_states_items()), + _ground_bullet_id=self._ground_bullet_id, + ) diff --git a/smarts/core/sumo_road_network.py b/smarts/core/sumo_road_network.py index d9778e231c..4f953d5f15 100644 --- a/smarts/core/sumo_road_network.py +++ b/smarts/core/sumo_road_network.py @@ -23,7 +23,7 @@ from functools import lru_cache from pathlib import Path from subprocess import check_output -from typing import Any, Dict, List, Optional, Sequence, Set, Tuple +from typing import Any, Dict, List, Optional, Sequence, Set, Tuple, Union import numpy as np from cached_property import cached_property @@ -140,7 +140,10 @@ def from_spec(cls, map_spec: MapSpec): junction_check_proc = multiprocessing.Process( target=cls._check_junctions, args=(net_file,), daemon=True ) - junction_check_proc.start() + try: + junction_check_proc.start() + except AssertionError: + cls._check_junctions(net_file) # Connections to internal lanes are implicit. If `withInternal=True` is # set internal junctions and the connections from internal lanes are @@ -168,7 +171,8 @@ def from_spec(cls, map_spec: MapSpec): # coordinates are relative to the origin). G._shifted_by_smarts = True - junction_check_proc.join(5) + if junction_check_proc.is_alive(): + junction_check_proc.join(5) return cls(G, net_file, map_spec) def _load_traffic_lights(self): @@ -203,7 +207,13 @@ def _map_path(map_spec: MapSpec) -> str: return os.path.join(map_spec.source, "map.net.xml") return map_spec.source - def is_same_map(self, map_spec: MapSpec) -> bool: + def is_same_map(self, map_or_spec: Union[MapSpec, RoadMap]) -> bool: + if isinstance(map_or_spec, SumoRoadNetwork): + map_spec = map_or_spec._map_spec + elif isinstance(map_or_spec, MapSpec): + map_spec = map_or_spec + else: + return False return ( ( map_spec.source == self._map_spec.source diff --git a/smarts/core/sumo_traffic_simulation.py b/smarts/core/sumo_traffic_simulation.py index b80465366f..63b569ad30 100644 --- a/smarts/core/sumo_traffic_simulation.py +++ b/smarts/core/sumo_traffic_simulation.py @@ -624,9 +624,9 @@ def _sync(self, provider_state: ProviderState): def _color_for_role(role: ActorRole) -> np.ndarray: if role == ActorRole.EgoAgent: return np.array(SceneColors.Agent.value[:3]) * 255 - if role == ActorRole.SocialAgent: + elif role == ActorRole.SocialAgent: return np.array(SceneColors.SocialAgent.value[:3]) * 255 - if role == ActorRole.Social: + elif role == ActorRole.Social: return np.array(SceneColors.SocialVehicle.value[:3]) * 255 return np.array(SceneColors.SocialVehicle.value[:3]) * 255 @@ -706,7 +706,7 @@ def _decode_tls_state(self, tls_state: str) -> SignalLightState: return SignalLightState.UNKNOWN def _create_signal_state( - self, sig_id: str, controlled_links: Sequence[Tuple[str, str, int]] + self, sig_id: str, controlled_links: Sequence[Tuple[str, str, str]] ) -> SignalState: incoming_lane_id = None controlled_lanes = [] @@ -714,7 +714,7 @@ def _create_signal_state( in_lane_id, out_lane_id, via_id = link via_lane = self._scenario.road_map.lane_by_id(via_id) assert via_lane - controlled_lanes.append(via_lane) + controlled_lanes.append(via_id) assert not incoming_lane_id or incoming_lane_id == in_lane_id incoming_lane_id = in_lane_id incoming_lane = self._scenario.road_map.lane_by_id(incoming_lane_id) diff --git a/smarts/core/tests/helpers/scenario.py b/smarts/core/tests/helpers/scenario.py index 126255898b..97b9185e1c 100644 --- a/smarts/core/tests/helpers/scenario.py +++ b/smarts/core/tests/helpers/scenario.py @@ -38,3 +38,8 @@ def temp_scenario(name: str, map: str): generate_glb_from_sumo_file(str(scenario / "map.net.xml"), str(scenario)) yield scenario + + +def maps_dir(): + """Add a maps directory.""" + return Path(__file__).parent.parent / "maps" diff --git a/smarts/core/tests/maps/map.net.xml b/smarts/core/tests/maps/map.net.xml new file mode 100644 index 0000000000..b5deb87ee3 --- /dev/null +++ b/smarts/core/tests/maps/map.net.xml @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/smarts/core/tests/test_boids.py b/smarts/core/tests/test_boids.py index 19e81a0820..293d14e0a2 100644 --- a/smarts/core/tests/test_boids.py +++ b/smarts/core/tests/test_boids.py @@ -67,7 +67,6 @@ def scenarios(bubble, traffic_sim): begin=("west", lane_idx, 0), end=("east", lane_idx, "max"), ), - repeat_route=True, rate=50, actors={ t.TrafficActor("car"): 1, @@ -134,7 +133,7 @@ def test_boids(smarts, scenarios, bubble): for vehicle in index.vehicles: position = Point(vehicle.position) in_bubble = position.within(geometry.bubble) - is_shadowing = index.shadow_actor_id_from_vehicle_id(vehicle.id) is not None + is_shadowing = index.shadower_id_from_vehicle_id(vehicle.id) is not None is_agent_controlled = vehicle.id in index.agent_vehicle_ids() vehicle_id = ( @@ -145,13 +144,11 @@ def test_boids(smarts, scenarios, bubble): zone_steps = steps_driven_in_zones[vehicle_id] if position.within(geometry.bubble): zone_steps.in_bubble += 1 - hijacked_actor_ids.append(index.actor_id_from_vehicle_id(vehicle.id)) + hijacked_actor_ids.append(index.owner_id_from_vehicle_id(vehicle.id)) assert in_bubble and not is_shadowing and is_agent_controlled elif position.within(geometry.airlock_entry): zone_steps.airlock_entry += 1 - shadowed_actor_ids.append( - index.shadow_actor_id_from_vehicle_id(vehicle.id) - ) + shadowed_actor_ids.append(index.shadower_id_from_vehicle_id(vehicle.id)) assert not in_bubble and is_shadowing and not is_agent_controlled elif position.within(geometry.airlock_exit): zone_steps.airlock_exit += 1 diff --git a/smarts/core/tests/test_bubble_hijacking.py b/smarts/core/tests/test_bubble_hijacking.py index 65baba09ac..dfd9a9fce7 100644 --- a/smarts/core/tests/test_bubble_hijacking.py +++ b/smarts/core/tests/test_bubble_hijacking.py @@ -52,12 +52,12 @@ def bubbles(): return [ t.Bubble( - zone=t.PositionalZone(pos=(150, -60), size=(30, 30)), + zone=t.PositionalZone(pos=(110, -60), size=(15, 30)), margin=10, actor=actor, ), t.Bubble( - zone=t.PositionalZone(pos=(60, -60), size=(30, 30)), + zone=t.PositionalZone(pos=(50, -60), size=(20, 30)), margin=10, actor=actor, ), @@ -80,7 +80,6 @@ def scenarios(bubbles, num_vehicles, traffic_sim): begin=("edge-west-WE", lane, 0), end=("edge-east-WE", lane, "max"), ), - repeat_route=True, rate=10, actors={t.TrafficActor(name="car"): 1}, ) @@ -130,7 +129,7 @@ def test_bubble_hijacking(smarts, scenarios, bubbles, num_vehicles, traffic_sim) geometries = [bubble_geometry(b, smarts.road_map) for b in bubbles] # bubble: vehicle: steps per zone - steps_driven_in_zones = {b.id: defaultdict(lambda: ZoneSteps()) for b in bubbles} + steps_driven_in_zones = {b.id: defaultdict(ZoneSteps) for b in bubbles} vehicles_made_to_through_bubble = {b.id: [] for b in bubbles} for _ in range(300): smarts.step({}) @@ -138,9 +137,7 @@ def test_bubble_hijacking(smarts, scenarios, bubbles, num_vehicles, traffic_sim) position = Point(vehicle.position) for bubble, geometry in zip(bubbles, geometries): in_bubble = position.within(geometry.bubble) - is_shadowing = ( - index.shadow_actor_id_from_vehicle_id(vehicle.id) is not None - ) + is_shadowing = index.shadower_id_from_vehicle_id(vehicle.id) is not None is_agent_controlled = vehicle.id in index.agent_vehicle_ids() vehicle_id = ( @@ -164,8 +161,8 @@ def test_bubble_hijacking(smarts, scenarios, bubbles, num_vehicles, traffic_sim) not in_bubble and not is_shadowing and not is_agent_controlled ) - # Just to have some padding, we want to be in each region at least 3 steps - min_steps = 3 + # Just to have some padding, we want to be in each region at least 2 steps + min_steps = 2 for bubble_id, zones in steps_driven_in_zones.items(): vehicle_ids = vehicles_made_to_through_bubble[bubble_id] assert ( @@ -175,10 +172,10 @@ def test_bubble_hijacking(smarts, scenarios, bubbles, num_vehicles, traffic_sim) zone = zones[vehicle_id] assert all( [ - zone.in_bubble > min_steps, - zone.outside_bubble > min_steps, - zone.airlock_entry > min_steps, - zone.airlock_exit > min_steps, + zone.in_bubble >= min_steps, + zone.outside_bubble >= min_steps, + zone.airlock_entry >= min_steps, + zone.airlock_exit >= min_steps, ] ), ( f"bubble={bubble_id}, vehicle_id={vehicle_id}, zone={zone} doesn't meet " diff --git a/smarts/core/tests/test_bubble_manager.py b/smarts/core/tests/test_bubble_manager.py index 0aa5b7cc5c..1598fd477b 100644 --- a/smarts/core/tests/test_bubble_manager.py +++ b/smarts/core/tests/test_bubble_manager.py @@ -36,6 +36,7 @@ from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation from smarts.core.tests.helpers.providers import MockProvider, MockTrafficProvider +from smarts.core.vehicle_index import VehicleIndex from smarts.sstudio import gen_scenario HEADING_CONSTANT = Heading(-math.pi / 2) @@ -273,7 +274,7 @@ def test_bubble_manager_state_change( ], ): state_at_position, _, _ = transition_cases - index = smarts.vehicle_index + index: VehicleIndex = smarts.vehicle_index vehicle_id = "vehicle" @@ -287,7 +288,7 @@ def test_bubble_manager_state_change( # Providers must be disjoint if index.vehicle_is_hijacked(vehicle_id): mock_provider.clear_next_provider_state() - agent_id = index.actor_id_from_vehicle_id(vehicle_id) + agent_id = index.owner_id_from_vehicle_id(vehicle_id) interface = smarts.agent_manager.agent_interface_for_agent_id(agent_id) while ( index.vehicle_is_hijacked(vehicle_id) diff --git a/smarts/core/tests/test_done_criteria.py b/smarts/core/tests/test_done_criteria.py index 150a94f031..4554bc3dd7 100644 --- a/smarts/core/tests/test_done_criteria.py +++ b/smarts/core/tests/test_done_criteria.py @@ -29,7 +29,7 @@ ) from smarts.core.scenario import Scenario from smarts.core.sensors import Sensors -from smarts.core.smarts import SMARTS +from smarts.core.smarts import SMARTS, SimulationFrame from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation AGENT1 = "agent1" @@ -92,25 +92,33 @@ def test_agents_alive_done_check(sim, scenario): sim.setup(scenario) interface = sim.agent_manager.agent_interface_for_agent_id(AGENT1) done_criteria = interface.done_criteria + + sim_frame: SimulationFrame = sim.cached_frame # 3 agents available, done requires 2 to be alive assert not Sensors._agents_alive_done_check( - sim.agent_manager, done_criteria.agents_alive + sim_frame.ego_ids, sim_frame.potential_agent_ids, done_criteria.agents_alive ) sim.agent_manager.teardown_ego_agents({AGENT2}) + del sim.cached_frame + sim_frame: SimulationFrame = sim.cached_frame # 2 agents available, done requires 2 to be alive assert not Sensors._agents_alive_done_check( - sim.agent_manager, done_criteria.agents_alive + sim_frame.ego_ids, sim_frame.potential_agent_ids, done_criteria.agents_alive ) sim.agent_manager.teardown_ego_agents({AGENT3}) + del sim.cached_frame + sim_frame: SimulationFrame = sim.cached_frame # 1 agents available, done requires 2 to be alive assert Sensors._agents_alive_done_check( - sim.agent_manager, done_criteria.agents_alive + sim_frame.ego_ids, sim_frame.potential_agent_ids, done_criteria.agents_alive ) sim.agent_manager.teardown_ego_agents({AGENT1}) + del sim.cached_frame + sim_frame: SimulationFrame = sim.cached_frame # 1 agents available, done requires 2 to be alive assert Sensors._agents_alive_done_check( - sim.agent_manager, done_criteria.agents_alive + sim_frame.ego_ids, sim_frame.potential_agent_ids, done_criteria.agents_alive ) diff --git a/smarts/core/tests/test_motion_planner_provider.py b/smarts/core/tests/test_motion_planner_provider.py index be65d7ffe3..fcb3db601a 100644 --- a/smarts/core/tests/test_motion_planner_provider.py +++ b/smarts/core/tests/test_motion_planner_provider.py @@ -24,7 +24,6 @@ import numpy as np import pytest -from smarts.core.actor import ActorRole from smarts.core.agent_interface import ActionSpaceType, AgentInterface, DoneCriteria from smarts.core.coordinates import Heading from smarts.core.plan import EndlessGoal, Mission, Start diff --git a/smarts/core/tests/test_parallel_sensors.py b/smarts/core/tests/test_parallel_sensors.py new file mode 100644 index 0000000000..049fb2e0dc --- /dev/null +++ b/smarts/core/tests/test_parallel_sensors.py @@ -0,0 +1,137 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import Any + +import pytest + +from smarts.core.agent_interface import AgentInterface, AgentType +from smarts.core.controllers import ActionSpaceType +from smarts.core.plan import Mission +from smarts.core.scenario import Scenario +from smarts.core.sensors.local_sensor_resolver import LocalSensorResolver +from smarts.core.sensors.parallel_sensor_resolver import ParallelSensorResolver +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.smarts import SMARTS +from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation +from smarts.core.utils.file import unpack +from smarts.core.utils.logging import diff_unpackable + +AGENT_IDS = [f"agent-00{i}" for i in range(3)] + + +@pytest.fixture +def scenario() -> Scenario: + s = Scenario( + scenario_root="scenarios/sumo/loop", + traffic_specs=["scenarios/sumo/loop/build/traffic/basic.rou.xml"], + missions=dict( + zip( + AGENT_IDS, + Scenario.discover_agent_missions( + scenario_root="scenarios/sumo/loop", + agents_to_be_briefed=AGENT_IDS, + ), + ) + ), + ) + missions = [ + Mission.random_endless_mission( + s.road_map, + ) + for _ in AGENT_IDS + ] + s.set_ego_missions(dict(zip(AGENT_IDS, missions))) + return s + + +@pytest.fixture( + params=[ + AgentInterface.from_type( + AgentType.Laner, + action=ActionSpaceType.Continuous, + ), + AgentInterface.from_type( + AgentType.Full, + action=ActionSpaceType.Continuous, + ), + ] +) +def sim(scenario, request): + a_interface = getattr(request, "param") + agents = {aid: a_interface for aid in AGENT_IDS} + smarts = SMARTS( + agents, + traffic_sims=[SumoTrafficSimulation()], + envision=None, + ) + smarts.reset(scenario) + smarts.step({aid: [0, 0, 0] for aid in AGENT_IDS}) + yield smarts + smarts.destroy() + + +def test_sensor_parallelization( + sim: SMARTS, +): + del sim.cached_frame + simulation_frame: SimulationFrame = sim.cached_frame + simulation_local_constants: SimulationLocalConstants = sim.local_constants + + parallel_resolver = ParallelSensorResolver(process_count_override=1) + serial_resolver = LocalSensorResolver() + + parallel_resolver.get_workers(1, sim_local_constants=sim.local_constants) + + assert len(simulation_frame.agent_ids) > 0 + p_observations, p_dones, p_updated_sensors = parallel_resolver.observe( + sim_frame=simulation_frame, + sim_local_constants=simulation_local_constants, + agent_ids=simulation_frame.agent_ids, + renderer=sim.renderer, + bullet_client=sim.bc, + ) + + l_observations, l_dones, l_updated_sensors = serial_resolver.observe( + sim_frame=simulation_frame, + sim_local_constants=simulation_local_constants, + agent_ids=simulation_frame.agent_ids, + renderer=sim.renderer, + bullet_client=sim.bc, + ) + + assert len(p_observations) > 0 + assert diff_unpackable(p_observations, l_observations) == "" + + assert len(p_dones) > 0 + assert diff_unpackable(p_dones, l_dones) == "" + + assert len(p_updated_sensors) > 0 + assert p_updated_sensors.keys() == l_updated_sensors.keys() + assert set(p_updated_sensors.keys()) not in simulation_frame.agent_ids + + # TODO: Make sure that all mutable sensors are returned + for agent_id, p_sensors in p_updated_sensors.items(): + assert len(p_sensors) > 0 + assert p_sensors.keys() == l_updated_sensors[agent_id].keys() + for k in p_sensors: + assert p_sensors[k] == l_updated_sensors[agent_id][k] diff --git a/smarts/core/tests/test_renderers.py b/smarts/core/tests/test_renderers.py index f03a121a7e..0eb36cecd9 100644 --- a/smarts/core/tests/test_renderers.py +++ b/smarts/core/tests/test_renderers.py @@ -104,7 +104,7 @@ def __init__(self, r, scenario, renderer_debug_mode: str, num_steps=3): from smarts.core.renderer import DEBUG_MODE as RENDERER_DEBUG_MODE from smarts.core.renderer import Renderer - self._rdr = Renderer( + self._renderer = Renderer( self._rid, RENDERER_DEBUG_MODE[renderer_debug_mode.upper()] ) except ImportError as e: @@ -117,22 +117,22 @@ def __init__(self, r, scenario, renderer_debug_mode: str, num_steps=3): self._vid = "r{}_car".format(r) def test_renderer(self): - self._rdr.setup(self._scenario) + self._renderer.setup(self._scenario) pose = Pose( position=np.array([71.65, 53.78, 0]), orientation=np.array([0, 0, 0, 0]), heading_=Heading(math.pi * 0.91), ) - self._rdr.create_vehicle_node( + self._renderer.create_vehicle_node( "simple_car.glb", self._vid, SceneColors.SocialVehicle, pose ) - self._rdr.begin_rendering_vehicle(self._vid, is_agent=False) + self._renderer.begin_rendering_vehicle(self._vid, is_agent=False) for s in range(self._num_steps): - self._rdr.render() + self._renderer.render() pose.position[0] = pose.position[0] + s pose.position[1] = pose.position[1] + s - self._rdr.update_vehicle_node(self._vid, pose) - self._rdr.remove_vehicle_node(self._vid) + self._renderer.update_vehicle_node(self._vid, pose) + self._renderer.remove_vehicle_node(self._vid) def test_multiple_renderers(scenario, request): diff --git a/smarts/core/tests/test_sensor_worker.py b/smarts/core/tests/test_sensor_worker.py new file mode 100644 index 0000000000..c687df33f6 --- /dev/null +++ b/smarts/core/tests/test_sensor_worker.py @@ -0,0 +1,136 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import Any + +import pytest + +from smarts.core.agent_interface import AgentInterface, AgentType +from smarts.core.controllers import ActionSpaceType +from smarts.core.plan import Mission +from smarts.core.scenario import Scenario +from smarts.core.sensors import Observation +from smarts.core.sensors.parallel_sensor_resolver import ( + SensorsWorker, + SensorsWorkerRequestId, + WorkerKwargs, +) +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.smarts import SMARTS +from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation +from smarts.core.utils.logging import diff_unpackable + +SimulationState = SimulationFrame +SensorState = Any + +AGENT_IDS = [f"agent-00{i}" for i in range(100)] + + +@pytest.fixture +def scenario() -> Scenario: + s = Scenario( + scenario_root="scenarios/sumo/loop", + traffic_specs=["scenarios/sumo/loop/build/traffic/basic.rou.xml"], + missions=dict( + zip( + AGENT_IDS, + Scenario.discover_agent_missions( + scenario_root="scenarios/sumo/loop", + agents_to_be_briefed=AGENT_IDS, + ), + ) + ), + ) + missions = [ + Mission.random_endless_mission( + s.road_map, + ) + for _ in AGENT_IDS + ] + s.set_ego_missions(dict(zip(AGENT_IDS, missions))) + return s + + +@pytest.fixture( + params=[ + AgentInterface.from_type( + AgentType.Laner, + action=ActionSpaceType.Continuous, + ), + AgentInterface.from_type( + AgentType.Full, + action=ActionSpaceType.Continuous, + ), + ] +) +def sim(scenario, request): + a_interface = getattr(request, "param") + agents = {aid: a_interface for aid in AGENT_IDS} + smarts = SMARTS( + agents, + traffic_sims=[SumoTrafficSimulation(headless=True)], + envision=None, + ) + smarts.reset(scenario) + smarts.step({aid: [0, 0, 0] for aid in AGENT_IDS}) + yield smarts + smarts.destroy() + + +def test_sensor_worker( + sim: SMARTS, +): + del sim.cached_frame + sim_frame: SimulationFrame = sim.cached_frame + agent_ids = set(AGENT_IDS) + worker = SensorsWorker() + worker.run() + worker.send( + request=SensorsWorker.Request( + SensorsWorkerRequestId.SIMULATION_LOCAL_CONSTANTS, + WorkerKwargs(sim_local_constants=sim.local_constants), + ) + ) + assert worker.running + worker_args = WorkerKwargs(sim_frame=sim_frame, agent_ids=agent_ids) + worker.send( + SensorsWorker.Request(SensorsWorkerRequestId.SIMULATION_FRAME, worker_args) + ) + state = dict( + sim_frame=sim_frame, + sim_local_constants=sim.local_constants, + agent_ids=agent_ids, + ) + observations, dones, updated_sensors = SensorsWorker.local(state=state) + other_observations, other_dones, updated_sensors = worker.result(timeout=5) + + assert isinstance(observations, dict) + assert all( + [isinstance(obs, Observation) for obs in observations.values()] + ), f"{observations}" + assert isinstance(dones, dict) + assert all([isinstance(obs, bool) for obs in dones.values()]) + assert isinstance(other_observations, dict) + assert all([isinstance(obs, Observation) for obs in other_observations.values()]) + assert isinstance(other_dones, dict) + assert all([isinstance(obs, bool) for obs in other_dones.values()]) + assert observations.keys() == other_dones.keys() + assert diff_unpackable(other_observations, observations) == "" diff --git a/smarts/core/tests/test_sensors.py b/smarts/core/tests/test_sensors.py index 17fa7a627d..dc8d976725 100644 --- a/smarts/core/tests/test_sensors.py +++ b/smarts/core/tests/test_sensors.py @@ -41,18 +41,20 @@ def test_driven_path_sensor(): sim = mock.Mock() max_path_length = 5 - sensor = DrivenPathSensor(vehicle, max_path_length=max_path_length) + sensor = DrivenPathSensor(max_path_length=max_path_length) positions = [(x, 0, 0) for x in range(0, 100, 10)] sim_times = list(range(0, 50, 5)) for idx, (position, sim_time) in enumerate(zip(positions, sim_times)): sim.elapsed_sim_time = sim_time - vehicle.position = position - sensor.track_latest_driven_path(sim) + vehicle.pose.position = position + sensor.track_latest_driven_path(sim.elapsed_sim_time, vehicle) if idx >= 3: - assert sensor.distance_travelled(sim, last_n_steps=3) == 30 - assert sensor.distance_travelled(sim, last_n_seconds=10) == 20 + assert sensor.distance_travelled(sim.elapsed_sim_time, last_n_steps=3) == 30 + assert ( + sensor.distance_travelled(sim.elapsed_sim_time, last_n_seconds=10) == 20 + ) assert len(sensor()) <= max_path_length @@ -84,32 +86,34 @@ def test_trip_meter_sensor(scenarios): scenario: Scenario = next(scenarios) sim = mock.Mock() - vehicle = mock.Mock() - vehicle.pose = Pose( + vehicle_state = mock.Mock() + vehicle_state.pose = Pose( position=np.array([33, -65, 0]), orientation=np.array([0, 0, 0, 0]), heading_=Heading(0), ) - vehicle.length = 3.68 + vehicle_state.length = 3.68 mission = scenario.missions[AGENT_ID] plan = Plan(scenario.road_map, mission) - sensor = TripMeterSensor(vehicle, scenario.road_map, plan) - waypoints_sensor = WaypointsSensor(vehicle, plan) + sensor = TripMeterSensor() + waypoints_sensor = WaypointsSensor() positions = [(x, 0, 0) for x in range(0, 100, 10)] sim_times = list(range(0, 50, 5)) for idx, (position, sim_time) in enumerate(zip(positions, sim_times)): sim.elapsed_sim_time = sim_time - vehicle.position = position - vehicle.pose = Pose( - position=vehicle.position, + vehicle_state.position = position + vehicle_state.pose = Pose( + position=vehicle_state.position, orientation=np.array([0, 0, 0, 0]), heading_=Heading(0), ) - waypoint_paths = waypoints_sensor() - sensor.update_distance_wps_record(waypoint_paths=waypoint_paths) + waypoint_paths = waypoints_sensor(vehicle_state, plan, scenario.road_map) + sensor.update_distance_wps_record( + waypoint_paths, vehicle_state, plan, sim.road_map + ) assert sensor() == sum( wpf.dist_to(wps.pos) @@ -121,9 +125,8 @@ def test_trip_meter_sensor(scenarios): def test_waypoints_sensor(scenarios): scenario = next(scenarios) - sim = mock.Mock() - vehicle = mock.Mock() - vehicle.pose = Pose( + vehicle_state = mock.Mock() + vehicle_state.pose = Pose( position=np.array([33, -65, 0]), orientation=np.array([0, 0, 0, 0]), heading_=Heading(0), @@ -132,8 +135,8 @@ def test_waypoints_sensor(scenarios): mission = scenario.missions[AGENT_ID] plan = Plan(scenario.road_map, mission) - sensor = WaypointsSensor(vehicle, plan) - waypoints = sensor() + sensor = WaypointsSensor() + waypoints = sensor(vehicle_state, plan, scenario.road_map) assert len(waypoints) == 3 diff --git a/smarts/core/tests/test_simulation_state_frame.py b/smarts/core/tests/test_simulation_state_frame.py new file mode 100644 index 0000000000..dca791478d --- /dev/null +++ b/smarts/core/tests/test_simulation_state_frame.py @@ -0,0 +1,103 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import List + +import pytest + +import smarts.core.serialization.default as serializer +from smarts.core.agent_interface import AgentInterface, AgentType +from smarts.core.scenario import Scenario +from smarts.core.smarts import SMARTS, SimulationFrame +from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation + +AGENT_ID = "agent-007" + + +@pytest.fixture +def agents_to_be_briefed(): + return [AGENT_ID] + + +@pytest.fixture() +def sim(): + agents = {AGENT_ID: AgentInterface.from_type(AgentType.Full)} + smarts = SMARTS( + agents, + traffic_sims=[SumoTrafficSimulation(headless=True)], + envision=None, + ) + + yield smarts + smarts.destroy() + + +@pytest.fixture +def scenario(agents_to_be_briefed: List[str]) -> Scenario: + return Scenario( + scenario_root="scenarios/sumo/loop", + traffic_specs=["scenarios/sumo/loop/build/traffic/basic.rou.xml"], + missions=dict( + zip( + agents_to_be_briefed, + Scenario.discover_agent_missions( + scenario_root="scenarios/sumo/loop", + agents_to_be_briefed=agents_to_be_briefed, + ), + ) + ), + ) + + +def test_state(sim: SMARTS, scenario): + sim.setup(scenario) + frame: SimulationFrame = sim.cached_frame + + assert isinstance(frame, SimulationFrame) + assert hasattr(frame, "actor_states") + assert hasattr(frame, "agent_interfaces") + assert hasattr(frame, "agent_vehicle_controls") + assert hasattr(frame, "ego_ids") + assert hasattr(frame, "elapsed_sim_time") + assert hasattr(frame, "fixed_timestep") + assert hasattr(frame, "resetting") + # This attribute was temporarily added but should not be here + assert not hasattr(frame, "road_map") + + assert hasattr(frame, "map_spec") + assert hasattr(frame, "last_dt") + assert hasattr(frame, "last_provider_state") + assert hasattr(frame, "step_count") + assert hasattr(frame, "vehicle_collisions") + + +def test_state_serialization(sim: SMARTS, scenario: Scenario): + sim.setup(scenario) + sim.reset(scenario, start_time=10) + sim.step({AGENT_ID: [0, 0, 0]}) + frame: SimulationFrame = sim.cached_frame + + # Check if serialization will work + serialized = serializer.dumps(frame) + deserialized: SimulationFrame = serializer.loads(serialized) + + # dataclass allows comparison + assert frame == deserialized diff --git a/smarts/core/tests/test_trap_manager.py b/smarts/core/tests/test_trap_manager.py index c82724a375..0e692b432c 100644 --- a/smarts/core/tests/test_trap_manager.py +++ b/smarts/core/tests/test_trap_manager.py @@ -174,7 +174,7 @@ def test_capture_vehicle(smarts: SMARTS, scenarios, traffic_sim): assert vehicle_id.startswith(vehicle_prefix) assert smarts.elapsed_sim_time < 1 assert len(smarts.vehicle_index.agent_vehicle_ids()) == 1 - assert smarts.vehicle_index.actor_id_from_vehicle_id(vehicle_id).startswith( + assert smarts.vehicle_index.owner_id_from_vehicle_id(vehicle_id).startswith( AGENT_ID ) @@ -184,7 +184,7 @@ def test_emit_on_default(smarts: SMARTS, empty_scenarios): smarts.reset(next(empty_scenarios)) assert round(smarts.elapsed_sim_time, 2) == 3.1 assert len(smarts.vehicle_index.agent_vehicle_ids()) == 1 - assert len(smarts.vehicle_index.vehicle_ids_by_actor_id(AGENT_ID)) == 1 + assert len(smarts.vehicle_index.vehicle_ids_by_owner_id(AGENT_ID)) == 1 def test_no_recapture_agent( diff --git a/smarts/core/traffic_history_provider.py b/smarts/core/traffic_history_provider.py index 464adbe963..f9d16dfc35 100644 --- a/smarts/core/traffic_history_provider.py +++ b/smarts/core/traffic_history_provider.py @@ -195,7 +195,7 @@ def step( # XXX: note that tls.lane_id may or may not correspond to a lane_id in the RoadMap # Here we assume that it will at least be part of the naming scheme somehow. if str(tls.lane_id) in feat_lane.lane_id: - controlled_lanes.append(feat_lane) + controlled_lanes.append(feat_lane.lane_id) signals.append( SignalState( actor_id=actor_id, @@ -209,7 +209,7 @@ def step( ) ) except: - pass + raise return ProviderState(actors=vehicles + signals) diff --git a/smarts/core/utils/file.py b/smarts/core/utils/file.py index dd39ad69e3..67cd03af5e 100644 --- a/smarts/core/utils/file.py +++ b/smarts/core/utils/file.py @@ -23,9 +23,12 @@ import pickle import shutil import struct +from collections import defaultdict from contextlib import contextmanager from ctypes import c_int64 -from typing import Any, Generator +from typing import Any, Generator, Sequence + +import numpy as np import smarts @@ -69,20 +72,21 @@ def isdataclass(x): return dataclasses.is_dataclass(x) +# TAI MTA: This is probably the wrong place for this utility: `logging.py`? def unpack(obj): - """A helper that can be used to print `nestedtuples`. For example, + """A helper that can be used to print nested data objects (`tuple`, `dataclass`, `namedtuple`, ...). For example, ```python pprint(unpack(obs), indent=1, width=80, compact=True) ``` """ if isinstance(obj, dict): return {key: unpack(value) for key, value in obj.items()} - elif isinstance(obj, list): + elif isinstance(obj, (list, np.ndarray)): return [unpack(value) for value in obj] elif isnamedtupleinstance(obj): - return unpack(obj._asdict()) + return {key: unpack(value) for key, value in obj._asdict().items()} elif isdataclass(obj): - return unpack(dataclasses.asdict(obj)) + return {key: unpack(value) for key, value in dataclasses.asdict(obj).items()} elif isinstance(obj, tuple): return tuple(unpack(value) for value in obj) else: diff --git a/smarts/core/utils/import_utils.py b/smarts/core/utils/import_utils.py index 89c8e6be63..866e5994e1 100644 --- a/smarts/core/utils/import_utils.py +++ b/smarts/core/utils/import_utils.py @@ -24,8 +24,8 @@ from pathlib import Path -def import_module_from_file(module_name, path: Path): - """Attempt to load a module dynamically from a file.""" +def import_module_from_file(module_name: str, path: Path): + """Attempt to load a module dynamically from a path as `module_name`.""" path = str(path) # Get one directory up if path not in sys.path: sys.path.append(path) diff --git a/smarts/core/utils/logging.py b/smarts/core/utils/logging.py index 0f2b381ce6..a88a1f9658 100644 --- a/smarts/core/utils/logging.py +++ b/smarts/core/utils/logging.py @@ -22,9 +22,13 @@ import os import sys import warnings +from collections import defaultdict from contextlib import contextmanager from io import UnsupportedOperation from time import time +from typing import Sequence + +from .file import unpack @contextmanager @@ -162,3 +166,50 @@ def suppress_websocket(): _logger.addFilter(websocket_filter) yield _logger.removeFilter(websocket_filter) + + +def diff_unpackable(obj, other_obj): + """Do an asserted comparision of an object that is able to be unpacked. This works with nested collections: + dictionaries, namedtuples, tuples, lists, numpy arrays, and dataclasses. + Raises: + AssertionError: if objects do not match. + """ + obj_unpacked = unpack(obj) + other_obj_unpacked = unpack(other_obj) + + def sort(orig_value): + value = orig_value + if isinstance(value, (dict, defaultdict)): + return dict(sorted(value.items(), key=lambda item: item[0])) + try: + s = sorted(value, key=lambda item: item[0]) + except IndexError: + s = sorted(value) + except (KeyError, TypeError): + s = value + return s + + def process(obj, other_obj, current_comparison): + if isinstance(obj, (dict, defaultdict)): + t_o = sort(obj) + assert isinstance(t_o, dict) + t_oo = sort(other_obj) + assert isinstance(t_oo, dict) + comps.append((t_o.keys(), t_oo.keys())) + comps.append((t_o.values(), t_oo.values())) + elif isinstance(obj, Sequence) and not isinstance(obj, (str)): + comps.append((sort(obj), sort(other_obj))) + elif obj != other_obj: + return f"{obj}!={other_obj} in {current_comparison}" + return "" + + comps = [] + result = process(obj_unpacked, other_obj_unpacked, None) + while len(comps) > 0: + o_oo = comps.pop() + for o, oo in zip(*o_oo): + if result != "": + return result + result = process(o, oo, o_oo) + + return "" diff --git a/smarts/core/utils/tests/fixtures.py b/smarts/core/utils/tests/fixtures.py index 0eb560b340..357cef105e 100644 --- a/smarts/core/utils/tests/fixtures.py +++ b/smarts/core/utils/tests/fixtures.py @@ -200,7 +200,6 @@ def large_observation(): ), drivable_area_grid_map=DrivableAreaGridMap( metadata=GridMapMetadata( - created_at=1649853761, resolution=0.1953125, width=256, height=256, @@ -221,7 +220,6 @@ def large_observation(): ), occupancy_grid_map=OccupancyGridMap( metadata=GridMapMetadata( - created_at=1649853761, resolution=0.1953125, width=256, height=256, @@ -242,7 +240,6 @@ def large_observation(): ), top_down_rgb=TopDownRGB( metadata=GridMapMetadata( - created_at=1649853761, resolution=0.1953125, width=256, height=256, diff --git a/smarts/core/vehicle.py b/smarts/core/vehicle.py index ee53a0b9b1..890f04014c 100644 --- a/smarts/core/vehicle.py +++ b/smarts/core/vehicle.py @@ -25,15 +25,12 @@ from typing import Any, Dict, List, Optional, Tuple, Union import numpy as np -from shapely.affinity import rotate as shapely_rotate -from shapely.geometry import Polygon -from shapely.geometry import box as shapely_box from smarts.core.agent_interface import AgentInterface from smarts.core.plan import Mission, Plan from . import models -from .actor import ActorRole, ActorState +from .actor import ActorRole from .chassis import AckermannChassis, BoxChassis, Chassis from .colors import SceneColors from .coordinates import Dimensions, Heading, Pose @@ -47,6 +44,7 @@ OGMSensor, RGBSensor, RoadWaypointsSensor, + Sensor, SignalsSensor, TripMeterSensor, ViaSensor, @@ -54,99 +52,7 @@ ) from .utils.custom_exceptions import RendererException from .utils.math import rotate_cw_around_point - - -@dataclass -class VehicleState(ActorState): - """Vehicle state information.""" - - vehicle_config_type: Optional[str] = None # key into VEHICLE_CONFIGS - pose: Optional[Pose] = None - dimensions: Optional[Dimensions] = None - speed: float = 0.0 - steering: Optional[float] = None - yaw_rate: Optional[float] = None - linear_velocity: Optional[np.ndarray] = None - angular_velocity: Optional[np.ndarray] = None - linear_acceleration: Optional[np.ndarray] = None - angular_acceleration: Optional[np.ndarray] = None - - def __post_init__(self): - assert self.pose is not None and self.dimensions is not None - - @property - def bbox(self) -> Polygon: - """Returns a bounding box around the vehicle.""" - pos = self.pose.point - half_len = 0.5 * self.dimensions.length - half_width = 0.5 * self.dimensions.width - poly = shapely_box( - pos.x - half_width, - pos.y - half_len, - pos.x + half_width, - pos.y + half_len, - ) - return shapely_rotate(poly, self.pose.heading, use_radians=True) - - -@dataclass(frozen=True) -class VehicleConfig: - """Vehicle configuration""" - - vehicle_type: str - color: SceneColors - dimensions: Dimensions - glb_model: str - - -# A mapping between SUMO's vehicle types and our internal vehicle config. -# TODO: Don't leak SUMO's types here. -# XXX: The GLB's dimensions must match the specified dimensions here. -# TODO: for traffic histories, vehicle (and road) dimensions are set by the dataset -VEHICLE_CONFIGS = { - "passenger": VehicleConfig( - vehicle_type="car", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=3.68, width=1.47, height=1.4), - glb_model="simple_car.glb", - ), - "bus": VehicleConfig( - vehicle_type="bus", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=7, width=2.25, height=3), - glb_model="bus.glb", - ), - "coach": VehicleConfig( - vehicle_type="coach", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=8, width=2.4, height=3.5), - glb_model="coach.glb", - ), - "truck": VehicleConfig( - vehicle_type="truck", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=5, width=1.91, height=1.89), - glb_model="truck.glb", - ), - "trailer": VehicleConfig( - vehicle_type="trailer", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=10, width=2.5, height=4), - glb_model="trailer.glb", - ), - "pedestrian": VehicleConfig( - vehicle_type="pedestrian", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=0.5, width=0.5, height=1.6), - glb_model="pedestrian.glb", - ), - "motorcycle": VehicleConfig( - vehicle_type="motorcycle", - color=SceneColors.SocialVehicle, - dimensions=Dimensions(length=2.5, width=1, height=1.4), - glb_model="motorcycle.glb", - ), -} +from .vehicle_state import VEHICLE_CONFIGS, VehicleConfig, VehicleState class Vehicle: @@ -178,14 +84,19 @@ def __init__( config = VEHICLE_CONFIGS[vehicle_config_type] self._color = config.color - self._renderer = None - self._initialized = True self._has_stepped = False def _assert_initialized(self): assert self._initialized, f"Vehicle({self.id}) is not initialized" + def __eq__(self, __o: object) -> bool: + if self is __o: + return True + if isinstance(__o, self.__class__) and self.state == __o.state: + return True + return False + def __repr__(self): return f"""Vehicle({self.id}, pose={self.pose}, @@ -235,17 +146,11 @@ def speed(self) -> float: return self._chassis.speed @property - def sensors(self) -> dict: + def sensors(self) -> Dict[str, Sensor]: """The sensors attached to this vehicle.""" self._assert_initialized() return self._sensors - @property - def renderer(self): # type: ignore - """The renderer this vehicle is rendered to.""" - # Returns: Optional[Renderer] - return self._renderer - # # TODO: See issue #898 This is a currently a no-op # @speed.setter # def speed(self, speed): @@ -459,118 +364,106 @@ def build_social_vehicle(sim, vehicle_id, vehicle_state: VehicleState) -> "Vehic ) @staticmethod - def attach_sensors_to_vehicle(sim, vehicle, agent_interface, plan): + def attach_sensors_to_vehicle( + sensor_manager, + sim, + vehicle: "Vehicle", + agent_interface, + ): """Attach sensors as required to satisfy the agent interface's requirements""" # The distance travelled sensor is not optional b/c it is used for the score # and reward calculation - vehicle.attach_trip_meter_sensor( - TripMeterSensor( - vehicle=vehicle, - road_map=sim.road_map, - plan=plan, - ) - ) + vehicle_state = vehicle.state + sensor = TripMeterSensor() + vehicle.attach_trip_meter_sensor(sensor) # The distance travelled sensor is not optional b/c it is used for visualization # done criteria - vehicle.attach_driven_path_sensor(DrivenPathSensor(vehicle=vehicle)) + sensor = DrivenPathSensor() + vehicle.attach_driven_path_sensor(sensor) if agent_interface.neighborhood_vehicle_states: - vehicle.attach_neighborhood_vehicle_states_sensor( - NeighborhoodVehiclesSensor( - vehicle=vehicle, - sim=sim, - radius=agent_interface.neighborhood_vehicle_states.radius, - ) + sensor = NeighborhoodVehiclesSensor( + radius=agent_interface.neighborhood_vehicle_states.radius, ) + vehicle.attach_neighborhood_vehicle_states_sensor(sensor) if agent_interface.accelerometer: - vehicle.attach_accelerometer_sensor(AccelerometerSensor(vehicle=vehicle)) + sensor = AccelerometerSensor() + vehicle.attach_accelerometer_sensor(sensor) if agent_interface.lane_positions: - vehicle.attach_lane_position_sensor(LanePositionSensor(vehicle=vehicle)) + sensor = LanePositionSensor() + vehicle.attach_lane_position_sensor(sensor) if agent_interface.waypoint_paths: - vehicle.attach_waypoints_sensor( - WaypointsSensor( - vehicle=vehicle, - plan=plan, - lookahead=agent_interface.waypoint_paths.lookahead, - ) + sensor = WaypointsSensor( + lookahead=agent_interface.waypoint_paths.lookahead, ) + vehicle.attach_waypoints_sensor(sensor) if agent_interface.road_waypoints: - vehicle.attach_road_waypoints_sensor( - RoadWaypointsSensor( - vehicle=vehicle, - sim=sim, - plan=plan, - horizon=agent_interface.road_waypoints.horizon, - ) + sensor = RoadWaypointsSensor( + horizon=agent_interface.road_waypoints.horizon, ) + vehicle.attach_road_waypoints_sensor(sensor) if agent_interface.drivable_area_grid_map: if not sim.renderer: raise RendererException.required_to("add a drivable_area_grid_map") - vehicle.attach_drivable_area_grid_map_sensor( - DrivableAreaGridMapSensor( - vehicle=vehicle, - width=agent_interface.drivable_area_grid_map.width, - height=agent_interface.drivable_area_grid_map.height, - resolution=agent_interface.drivable_area_grid_map.resolution, - renderer=sim.renderer, - ) + sensor = DrivableAreaGridMapSensor( + vehicle_state=vehicle_state, + width=agent_interface.drivable_area_grid_map.width, + height=agent_interface.drivable_area_grid_map.height, + resolution=agent_interface.drivable_area_grid_map.resolution, + renderer=sim.renderer, ) + vehicle.attach_drivable_area_grid_map_sensor(sensor) if agent_interface.occupancy_grid_map: if not sim.renderer: raise RendererException.required_to("add an OGM") - vehicle.attach_ogm_sensor( - OGMSensor( - vehicle=vehicle, - width=agent_interface.occupancy_grid_map.width, - height=agent_interface.occupancy_grid_map.height, - resolution=agent_interface.occupancy_grid_map.resolution, - renderer=sim.renderer, - ) + sensor = OGMSensor( + vehicle_state=vehicle_state, + width=agent_interface.occupancy_grid_map.width, + height=agent_interface.occupancy_grid_map.height, + resolution=agent_interface.occupancy_grid_map.resolution, + renderer=sim.renderer, ) + vehicle.attach_ogm_sensor(sensor) if agent_interface.top_down_rgb: if not sim.renderer: raise RendererException.required_to("add an RGB camera") - vehicle.attach_rgb_sensor( - RGBSensor( - vehicle=vehicle, - width=agent_interface.top_down_rgb.width, - height=agent_interface.top_down_rgb.height, - resolution=agent_interface.top_down_rgb.resolution, - renderer=sim.renderer, - ) + sensor = RGBSensor( + vehicle_state=vehicle_state, + width=agent_interface.top_down_rgb.width, + height=agent_interface.top_down_rgb.height, + resolution=agent_interface.top_down_rgb.resolution, + renderer=sim.renderer, ) + vehicle.attach_rgb_sensor(sensor) if agent_interface.lidar_point_cloud: - vehicle.attach_lidar_sensor( - LidarSensor( - vehicle=vehicle, - bullet_client=sim.bc, - sensor_params=agent_interface.lidar_point_cloud.sensor_params, - ) + sensor = LidarSensor( + vehicle_state=vehicle_state, + sensor_params=agent_interface.lidar_point_cloud.sensor_params, ) + vehicle.attach_lidar_sensor(sensor) - vehicle.attach_via_sensor( - ViaSensor( - vehicle=vehicle, - plan=plan, - # At lane change time of 6s and speed of 13.89m/s, acquistion range = 6s x 13.89m/s = 83.34m. - lane_acquisition_range=80, - speed_accuracy=1.5, - ) + sensor = ViaSensor( + # At lane change time of 6s and speed of 13.89m/s, acquistion range = 6s x 13.89m/s = 83.34m. + lane_acquisition_range=80, + speed_accuracy=1.5, ) + vehicle.attach_via_sensor(sensor) if agent_interface.signals: lookahead = agent_interface.signals.lookahead - vehicle.attach_signals_sensor( - SignalsSensor( - vehicle=vehicle, road_map=sim.road_map, lookahead=lookahead - ) - ) + sensor = SignalsSensor(lookahead=lookahead) + vehicle.attach_signals_sensor(sensor) + + for sensor_name, sensor in vehicle.sensors.items(): + if not sensor: + continue + sensor_manager.add_sensor_for_actor(vehicle.id, sensor_name, sensor) def step(self, current_simulation_time): """Update internal state.""" @@ -609,19 +502,12 @@ def update_state(self, state: VehicleState, dt: float): def create_renderer_node(self, renderer): """Create the vehicle's rendering node in the renderer.""" - assert not self._renderer - self._renderer = renderer config = VEHICLE_CONFIGS[self._vehicle_config_type] - self._renderer.create_vehicle_node( + return renderer.create_vehicle_node( config.glb_model, self._id, self.vehicle_color, self.pose ) - def sync_to_renderer(self): - """Update the vehicle's rendering node.""" - assert self._renderer - self._renderer.update_vehicle_node(self._id, self.pose) - - @lru_cache(maxsize=1) + # @lru_cache(maxsize=1) def _warn_AckermannChassis_set_pose(self): if self._has_stepped and isinstance(self._chassis, AckermannChassis): logging.warning( @@ -646,25 +532,12 @@ def swap_chassis(self, chassis: Chassis): self._chassis.teardown() self._chassis = chassis - def teardown(self, exclude_chassis=False): + def teardown(self, renderer, exclude_chassis=False): """Clean up internal resources""" - for sensor in [ - sensor - for sensor in [ - self._drivable_area_grid_map_sensor, - self._ogm_sensor, - self._rgb_sensor, - self._lidar_sensor, - self._driven_path_sensor, - ] - if sensor is not None - ]: - sensor.teardown() - if not exclude_chassis: self._chassis.teardown() - if self._renderer: - self._renderer.remove_vehicle_node(self._id) + if renderer: + renderer.remove_vehicle_node(self._id) self._initialized = False def _meta_create_sensor_functions(self): @@ -705,9 +578,9 @@ def attach_sensor(self, sensor, sensor_name=sensor_name): def detach_sensor(self, sensor_name=sensor_name): sensor = getattr(self, f"_{sensor_name}", None) if sensor is not None: - sensor.teardown() setattr(self, f"_{sensor_name}", None) del self._sensors[sensor_name] + return sensor def subscribed_to(self, sensor_name=sensor_name): sensor = getattr(self, f"_{sensor_name}", None) @@ -725,9 +598,11 @@ def sensor_property(self, sensor_name=sensor_name): setattr(Vehicle, f"{sensor_name}", property(sensor_property)) def detach_all_sensors_from_vehicle(vehicle): + sensors = [] for sensor_name in sensor_names: detach_sensor_func = getattr(vehicle, f"detach_{sensor_name}") - detach_sensor_func() + sensors.append(detach_sensor_func()) + return sensors setattr( Vehicle, diff --git a/smarts/core/vehicle_index.py b/smarts/core/vehicle_index.py index 8413ea5655..4eea651721 100644 --- a/smarts/core/vehicle_index.py +++ b/smarts/core/vehicle_index.py @@ -21,11 +21,12 @@ from copy import copy, deepcopy from io import StringIO from typing import ( + Dict, FrozenSet, Iterator, + List, NamedTuple, Optional, - Sequence, Set, Tuple, Union, @@ -61,10 +62,10 @@ def _2id(id_: str): class _ControlEntity(NamedTuple): vehicle_id: Union[bytes, str] - actor_id: Union[bytes, str] - actor_role: ActorRole - shadow_actor_id: Union[bytes, str] - # Applies to shadowing and controlling actor + owner_id: Union[bytes, str] + role: ActorRole + shadower_id: Union[bytes, str] + # Applies to shadower and owner # TODO: Consider moving this to an ActorRole field is_boid: bool is_hijacked: bool @@ -75,26 +76,23 @@ class _ControlEntity(NamedTuple): # VehicleIndex can perform operations on. Then we can do diffs of that # recarray with subset queries. class VehicleIndex: - """A vehicle management system that associates actors with vehicles.""" + """A vehicle management system that associates owners with vehicles.""" def __init__(self): self._log = logging.getLogger(self.__class__.__name__) self._controlled_by = VehicleIndex._build_empty_controlled_by() # Fixed-length ID to original ID - # TODO: This quitely breaks if actor and vehicle IDs are the same. It assumes + # TODO: This quitely breaks if owner and vehicle IDs are the same. It assumes # global uniqueness. self._2id_to_id = {} # {vehicle_id (fixed-length): } - self._vehicles = {} + self._vehicles: Dict[str, Vehicle] = {} # {vehicle_id (fixed-length): } self._controller_states = {} - # {vehicle_id (fixed-length): } - self._sensor_states = {} - # Loaded from yaml file on scenario reset self._controller_params = {} @@ -138,11 +136,6 @@ def _subset(self, vehicle_ids): for id_ in vehicle_ids if id_ in self._controller_states } - index._sensor_states = { - id_: self._sensor_states[id_] - for id_ in vehicle_ids - if id_ in self._sensor_states - } return index def __deepcopy__(self, memo): @@ -150,7 +143,7 @@ def __deepcopy__(self, memo): memo[id(self)] = result dict_ = copy(self.__dict__) - shallow = ["_2id_to_id", "_vehicles", "_sensor_states", "_controller_states"] + shallow = ["_2id_to_id", "_vehicles", "_controller_states"] for k in shallow: v = dict_.pop(k) setattr(result, k, copy(v)) @@ -170,8 +163,8 @@ def vehicle_ids(self) -> Set[str]: def agent_vehicle_ids(self) -> Set[str]: """A set of vehicle ids associated with an agent.""" vehicle_ids = self._controlled_by[ - (self._controlled_by["actor_role"] == ActorRole.EgoAgent) - | (self._controlled_by["actor_role"] == ActorRole.SocialAgent) + (self._controlled_by["role"] == ActorRole.EgoAgent) + | (self._controlled_by["role"] == ActorRole.SocialAgent) ]["vehicle_id"] return {self._2id_to_id[id_] for id_ in vehicle_ids} @@ -181,7 +174,7 @@ def social_vehicle_ids( ) -> Set[str]: """A set of vehicle ids associated with traffic vehicles.""" vehicle_ids = self._controlled_by[ - self._controlled_by["actor_role"] == ActorRole.Social + self._controlled_by["role"] == ActorRole.Social ]["vehicle_id"] return { self._2id_to_id[id_] @@ -191,7 +184,7 @@ def social_vehicle_ids( @cache def vehicle_is_hijacked_or_shadowed(self, vehicle_id) -> Tuple[bool, bool]: - """Determine if a vehicle is either taken over by an agent or watched by an agent.""" + """Determine if a vehicle is either taken over by an owner or watched.""" vehicle_id = _2id(vehicle_id) v_index = self._controlled_by["vehicle_id"] == vehicle_id @@ -202,65 +195,58 @@ def vehicle_is_hijacked_or_shadowed(self, vehicle_id) -> Tuple[bool, bool]: assert len(vehicle) == 1 vehicle = vehicle[0] - return bool(vehicle["is_hijacked"]), bool(vehicle["shadow_actor_id"]) + return bool(vehicle["is_hijacked"]), bool(vehicle["shadower_id"]) @cache - def vehicle_ids_by_actor_id(self, actor_id, include_shadowers=False): - """Returns all vehicles for the given actor ID as a list. This is most + def vehicle_ids_by_owner_id(self, owner_id, include_shadowers=False) -> List[str]: + """Returns all vehicles for the given owner ID as a list. This is most applicable when an agent is controlling multiple vehicles (e.g. with boids). """ - actor_id = _2id(actor_id) + owner_id = _2id(owner_id) - v_index = self._controlled_by["actor_id"] == actor_id + v_index = self._controlled_by["owner_id"] == owner_id if include_shadowers: - v_index = v_index | (self._controlled_by["shadow_actor_id"] == actor_id) + v_index = v_index | (self._controlled_by["shadower_id"] == owner_id) vehicle_ids = self._controlled_by[v_index]["vehicle_id"] return [self._2id_to_id[id_] for id_ in vehicle_ids] @cache - def actor_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: - """Find the actor id associated with the given vehicle.""" + def owner_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: + """Find the owner id associated with the given vehicle.""" vehicle_id = _2id(vehicle_id) - actor_ids = self._controlled_by[ + owner_ids = self._controlled_by[ self._controlled_by["vehicle_id"] == vehicle_id - ]["actor_id"] + ]["owner_id"] - if actor_ids: - return self._2id_to_id[actor_ids[0]] + if owner_ids: + return self._2id_to_id[owner_ids[0]] return None @cache - def shadow_actor_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: - """Find the first actor watching a vehicle.""" + def shadower_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: + """Find the first shadower watching a vehicle.""" vehicle_id = _2id(vehicle_id) - shadow_actor_ids = self._controlled_by[ + shadower_ids = self._controlled_by[ self._controlled_by["vehicle_id"] == vehicle_id - ]["shadow_actor_id"] + ]["shadower_id"] - if shadow_actor_ids: - return self._2id_to_id[shadow_actor_ids[0]] + if shadower_ids: + return self._2id_to_id[shadower_ids[0]] return None @cache - def shadower_ids_from_vehicle_id(self, vehicle_id) -> Sequence[str]: - """Find the first actor watching a vehicle.""" - vehicle_id = _2id(vehicle_id) - - shadow_actor_ids = self._controlled_by[ - self._controlled_by["vehicle_id"] == vehicle_id - ]["shadow_actor_id"] - - if shadow_actor_ids: - return [ - self._2id_to_id[shadow_actor_id] for shadow_actor_id in shadow_actor_ids - ] - - return [] + def shadower_ids(self) -> Set[str]: + """Get all current shadowers.""" + return set( + self._2id_to_id[sa_id] + for sa_id in self._controlled_by["shadower_id"] + if sa_id not in (b"", None) + ) @cache def vehicle_position(self, vehicle_id): @@ -273,26 +259,26 @@ def vehicle_position(self, vehicle_id): return positions[0] if len(positions) > 0 else None - def vehicles_by_actor_id(self, actor_id, include_shadowers=False): - """Find vehicles associated with the given actor. + def vehicles_by_owner_id(self, owner_id, include_shadowers=False): + """Find vehicles associated with the given owner id. Args: - actor_id: - The actor to find all associated vehicle ids. + owner_id: + The owner id to find all associated vehicle ids. include_shadowers: - If to include vehicles that the actor is only watching. + If to include vehicles that the owner is only watching. Returns: A list of associated vehicles. """ - vehicle_ids = self.vehicle_ids_by_actor_id(actor_id, include_shadowers) + vehicle_ids = self.vehicle_ids_by_owner_id(owner_id, include_shadowers) return [self._vehicles[_2id(id_)] for id_ in vehicle_ids] def vehicle_is_hijacked(self, vehicle_id: str) -> bool: - """Determine if a vehicle is controlled by an actor.""" + """Determine if a vehicle is controlled by an owner.""" is_hijacked, _ = self.vehicle_is_hijacked_or_shadowed(vehicle_id) return is_hijacked def vehicle_is_shadowed(self, vehicle_id: str) -> bool: - """Determine if a vehicle is watched by an actor.""" + """Determine if a vehicle is watched by an owner.""" _, is_shadowed = self.vehicle_is_hijacked_or_shadowed(vehicle_id) return is_shadowed @@ -315,7 +301,7 @@ def vehicle_by_id(self, vehicle_id, default=...): return self._vehicles.get(vehicle_id, default) @clear_cache - def teardown_vehicles_by_vehicle_ids(self, vehicle_ids): + def teardown_vehicles_by_vehicle_ids(self, vehicle_ids, renderer: Optional[object]): """Terminate and remove a vehicle from the index using its id.""" self._log.debug(f"Tearing down vehicle ids: {vehicle_ids}") @@ -326,14 +312,13 @@ def teardown_vehicles_by_vehicle_ids(self, vehicle_ids): for vehicle_id in vehicle_ids: vehicle = self._vehicles.pop(vehicle_id, None) if vehicle is not None: - vehicle.teardown() + vehicle.teardown(renderer=renderer) # popping since sensor_states/controller_states may not include the # vehicle if it's not being controlled by an agent - self._sensor_states.pop(vehicle_id, None) self._controller_states.pop(vehicle_id, None) - # TODO: This stores actors/agents as well; those aren't being cleaned-up + # TODO: This stores agents as well; those aren't being cleaned-up self._2id_to_id.pop(vehicle_id, None) remove_vehicle_indices = np.isin( @@ -342,15 +327,17 @@ def teardown_vehicles_by_vehicle_ids(self, vehicle_ids): self._controlled_by = self._controlled_by[~remove_vehicle_indices] - def teardown_vehicles_by_actor_ids(self, actor_ids, include_shadowing=True): - """Terminate and remove all vehicles associated with an actor.""" + def teardown_vehicles_by_owner_ids( + self, owner_ids, renderer, include_shadowing=True + ): + """Terminate and remove all vehicles associated with an owner id.""" vehicle_ids = [] - for actor_id in actor_ids: + for owner_id in owner_ids: vehicle_ids.extend( - [v.id for v in self.vehicles_by_actor_id(actor_id, include_shadowing)] + [v.id for v in self.vehicles_by_owner_id(owner_id, include_shadowing)] ) - self.teardown_vehicles_by_vehicle_ids(vehicle_ids) + self.teardown_vehicles_by_vehicle_ids(vehicle_ids, renderer) return vehicle_ids @@ -365,16 +352,15 @@ def sync(self): ) @clear_cache - def teardown(self): + def teardown(self, renderer): """Clean up resources, resetting the index.""" self._controlled_by = VehicleIndex._build_empty_controlled_by() for vehicle in self._vehicles.values(): - vehicle.teardown(exclude_chassis=True) + vehicle.teardown(renderer=renderer, exclude_chassis=True) self._vehicles = {} self._controller_states = {} - self._sensor_states = {} self._2id_to_id = {} @clear_cache @@ -386,13 +372,18 @@ def start_agent_observation( vehicle_id, agent_id = _2id(vehicle_id), _2id(agent_id) vehicle = self._vehicles[vehicle_id] - Vehicle.attach_sensors_to_vehicle(sim, vehicle, agent_interface, plan) + Vehicle.attach_sensors_to_vehicle( + sim.sensor_manager, sim, vehicle, agent_interface + ) self._2id_to_id[agent_id] = original_agent_id - self._sensor_states[vehicle_id] = SensorState( - agent_interface.max_episode_steps, - plan=plan, + sim.sensor_manager.add_sensor_state( + vehicle.id, + SensorState( + agent_interface.max_episode_steps, + plan_frame=plan.frame(), + ), ) self._controller_states[vehicle_id] = ControllerState.from_action_space( @@ -402,7 +393,7 @@ def start_agent_observation( v_index = self._controlled_by["vehicle_id"] == vehicle_id entity = _ControlEntity(*self._controlled_by[v_index][0]) self._controlled_by[v_index] = tuple( - entity._replace(shadow_actor_id=agent_id, is_boid=boid) + entity._replace(shadower_id=agent_id, is_boid=boid) ) # XXX: We are not giving the vehicle an AckermannChassis here but rather later @@ -434,7 +425,7 @@ def switch_control_to_agent( boid: If the agent is acting as a boid agent controlling multiple vehicles. hijacking: - If the vehicle has been taken over from another controlling actor. + If the vehicle has been taken over from another controlling owner. recreate: If the vehicle should be destroyed and regenerated. agent_interface: @@ -462,17 +453,16 @@ def switch_control_to_agent( dimensions=vehicle.state.dimensions, bullet_client=sim.bc, ) - vehicle.swap_chassis(chassis) v_index = self._controlled_by["vehicle_id"] == vehicle_id entity = _ControlEntity(*self._controlled_by[v_index][0]) - actor_role = ActorRole.SocialAgent if hijacking else ActorRole.EgoAgent + role = ActorRole.SocialAgent if hijacking else ActorRole.EgoAgent self._controlled_by[v_index] = tuple( entity._replace( - actor_role=actor_role, - actor_id=agent_id, - shadow_actor_id=b"", + role=role, + owner_id=agent_id, + shadower_id=b"", is_boid=boid, is_hijacked=hijacking, ) @@ -491,46 +481,46 @@ def stop_shadowing(self, shadower_id: str, vehicle_id: Optional[str] = None): """ shadower_id = _2id(shadower_id) - v_index = self._controlled_by["shadow_actor_id"] == shadower_id + v_index = self._controlled_by["shadower_id"] == shadower_id if vehicle_id: vehicle_id = _2id(vehicle_id) - # This multiplication finds overlap of "shadow_actor_id" and "vehicle_id" + # This multiplication finds overlap of "shadower_id" and "vehicle_id" v_index = (self._controlled_by["vehicle_id"] == vehicle_id) * v_index for entity in self._controlled_by[v_index]: entity = _ControlEntity(*entity) - self._controlled_by[v_index] = tuple(entity._replace(shadow_actor_id=b"")) + self._controlled_by[v_index] = tuple(entity._replace(shadower_id=b"")) @clear_cache - def stop_agent_observation(self, vehicle_id): - """Strip all sensors from a vehicle and stop all actors from watching the vehicle.""" + def stop_agent_observation(self, vehicle_id) -> Vehicle: + """Strip all sensors from a vehicle and stop all owners from watching the vehicle.""" vehicle_id = _2id(vehicle_id) vehicle = self._vehicles[vehicle_id] - # pytype: disable=attribute-error - Vehicle.detach_all_sensors_from_vehicle(vehicle) - # pytype: enable=attribute-error - # TAI: del self._sensor_states[vehicle_id] v_index = self._controlled_by["vehicle_id"] == vehicle_id entity = self._controlled_by[v_index][0] entity = _ControlEntity(*entity) - self._controlled_by[v_index] = tuple(entity._replace(shadow_actor_id=b"")) + self._controlled_by[v_index] = tuple(entity._replace(shadower_id="")) return vehicle @clear_cache def relinquish_agent_control( - self, sim, vehicle_id: str - ) -> Tuple[VehicleState, RoadMap.Route]: + self, sim, vehicle_id: str, road_map + ) -> Tuple[VehicleState, List[str]]: """Give control of the vehicle back to its original controller.""" self._log.debug(f"Relinquishing agent control v_id={vehicle_id}") v_id = _2id(vehicle_id) - ss = self._sensor_states[v_id] - route = ss.plan.route - self.stop_agent_observation(vehicle_id) + ss = sim.sensor_manager.sensor_state_for_actor_id(vehicle_id) + route = ss.get_plan(road_map).route + vehicle = self.stop_agent_observation(vehicle_id) + + # pytype: disable=attribute-error + Vehicle.detach_all_sensors_from_vehicle(vehicle) + # pytype: enable=attribute-error vehicle = self._vehicles[v_id] box_chassis = BoxChassis( @@ -546,9 +536,9 @@ def relinquish_agent_control( entity = _ControlEntity(*entity) self._controlled_by[v_index] = tuple( entity._replace( - actor_role=ActorRole.Social, - actor_id=b"", - shadow_actor_id=b"", + role=ActorRole.Social, + owner_id=b"", + shadower_id=b"", is_boid=False, is_hijacked=False, ) @@ -562,10 +552,18 @@ def attach_sensors_to_vehicle(self, sim, vehicle_id, agent_interface, plan): vehicle_id = _2id(vehicle_id) vehicle = self._vehicles[vehicle_id] - Vehicle.attach_sensors_to_vehicle(sim, vehicle, agent_interface, plan) - self._sensor_states[vehicle_id] = SensorState( - agent_interface.max_episode_steps, - plan=plan, + Vehicle.attach_sensors_to_vehicle( + sim.sensor_manager, + sim, + vehicle, + agent_interface, + ) + sim.sensor_manager.add_sensor_state( + vehicle.id, + SensorState( + agent_interface.max_episode_steps, + plan_frame=plan.frame, + ), ) self._controller_states[vehicle_id] = ControllerState.from_action_space( agent_interface.action, vehicle.pose, sim @@ -590,9 +588,9 @@ def _switch_control_to_agent_recreate( agent_interface is not None ), f"Missing agent_interface for agent_id={agent_id}" vehicle = self._vehicles[vehicle_id] - sensor_state = self._sensor_states[vehicle_id] + sensor_state = sim.sensor_manager.sensor_state_for_actor_id(vehicle.id) controller_state = self._controller_states[vehicle_id] - plan = sensor_state.plan + plan = sensor_state.get_plan(sim.road_map) # Create a new vehicle to replace the old one new_vehicle = Vehicle.build_agent_vehicle( @@ -617,7 +615,7 @@ def _switch_control_to_agent_recreate( ) # Remove the old vehicle - self.teardown_vehicles_by_vehicle_ids([vehicle.id]) + self.teardown_vehicles_by_vehicle_ids([vehicle.id], sim.renderer_ref) # HACK: Directly remove the vehicle from the traffic provider (should do this via the sim instead) for traffic_sim in sim.traffic_sims: if traffic_sim.manages_actor(vehicle.id): @@ -628,7 +626,7 @@ def _switch_control_to_agent_recreate( traffic_sim.stop_managing(vehicle.id) # Take control of the new vehicle - self._enfranchise_actor( + self._enfranchise_agent( sim, agent_id, agent_interface, @@ -667,16 +665,13 @@ def build_agent_vehicle( initial_speed=initial_speed, ) - sensor_state = SensorState( - agent_interface.max_episode_steps, - plan=plan, - ) + sensor_state = SensorState(agent_interface.max_episode_steps, plan.frame()) controller_state = ControllerState.from_action_space( agent_interface.action, vehicle.pose, sim ) - self._enfranchise_actor( + self._enfranchise_agent( sim, agent_id, agent_interface, @@ -690,7 +685,7 @@ def build_agent_vehicle( return vehicle @clear_cache - def _enfranchise_actor( + def _enfranchise_agent( self, sim, agent_id, @@ -705,27 +700,27 @@ def _enfranchise_actor( original_agent_id = agent_id Vehicle.attach_sensors_to_vehicle( - sim, vehicle, agent_interface, sensor_state.plan + sim.sensor_manager, sim, vehicle, agent_interface ) if sim.is_rendering: - vehicle.create_renderer_node(sim.renderer) + vehicle.create_renderer_node(sim.renderer_ref) sim.renderer.begin_rendering_vehicle(vehicle.id, is_agent=True) vehicle_id = _2id(vehicle.id) agent_id = _2id(original_agent_id) - self._sensor_states[vehicle_id] = sensor_state + sim.sensor_manager.add_sensor_state(vehicle.id, sensor_state) self._controller_states[vehicle_id] = controller_state self._vehicles[vehicle_id] = vehicle self._2id_to_id[vehicle_id] = vehicle.id self._2id_to_id[agent_id] = original_agent_id - actor_role = ActorRole.SocialAgent if hijacking else ActorRole.EgoAgent + role = ActorRole.SocialAgent if hijacking else ActorRole.EgoAgent entity = _ControlEntity( vehicle_id=vehicle_id, - actor_id=agent_id, - actor_role=actor_role, - shadow_actor_id=b"", + owner_id=agent_id, + role=role, + shadower_id=b"", is_boid=boid, is_hijacked=hijacking, position=vehicle.position, @@ -734,7 +729,7 @@ def _enfranchise_actor( @clear_cache def build_social_vehicle( - self, sim, vehicle_state, actor_id, vehicle_id=None + self, sim, vehicle_state, owner_id, vehicle_id=None ) -> Vehicle: """Build an entirely new vehicle for a social agent.""" if vehicle_id is None: @@ -746,24 +741,24 @@ def build_social_vehicle( vehicle_state, ) - vehicle_id, actor_id = _2id(vehicle_id), _2id(actor_id) + vehicle_id, owner_id = _2id(vehicle_id), _2id(owner_id) if owner_id else b"" if sim.is_rendering: - vehicle.create_renderer_node(sim.renderer) + vehicle.create_renderer_node(sim.renderer_ref) sim.renderer.begin_rendering_vehicle(vehicle.id, is_agent=False) self._vehicles[vehicle_id] = vehicle self._2id_to_id[vehicle_id] = vehicle.id - actor_role = vehicle_state.role - assert actor_role not in ( + role = vehicle_state.role + assert role not in ( ActorRole.EgoAgent, ActorRole.SocialAgent, - ), f"role={actor_role} from {vehicle_state.source}" + ), f"role={role} from {vehicle_state.source}" entity = _ControlEntity( vehicle_id=vehicle_id, - actor_id=actor_id, - actor_role=actor_role, - shadow_actor_id=b"", + owner_id=owner_id, + role=role, + shadower_id=b"", is_boid=False, is_hijacked=False, position=np.asarray(vehicle.position), @@ -774,27 +769,12 @@ def build_social_vehicle( def begin_rendering_vehicles(self, renderer): """Render vehicles using the specified renderer.""" - agent_ids = self.agent_vehicle_ids() + agent_vehicle_ids = self.agent_vehicle_ids() for vehicle in self._vehicles.values(): - if not vehicle.renderer: - vehicle.create_renderer_node(renderer) - is_agent = vehicle.id in agent_ids + if vehicle.create_renderer_node(renderer): + is_agent = vehicle.id in agent_vehicle_ids renderer.begin_rendering_vehicle(vehicle.id, is_agent) - def sensor_states_items(self): - """Get the sensor states of all listed vehicles.""" - return map(lambda x: (self._2id_to_id[x[0]], x[1]), self._sensor_states.items()) - - def check_vehicle_id_has_sensor_state(self, vehicle_id: str) -> bool: - """Determine if a vehicle contains sensors.""" - v_id = _2id(vehicle_id) - return v_id in self._sensor_states - - def sensor_state_for_vehicle_id(self, vehicle_id: str) -> SensorState: - """Retrieve the sensor state of the given vehicle.""" - vehicle_id = _2id(vehicle_id) - return self._sensor_states[vehicle_id] - @cache def controller_state_for_vehicle_id(self, vehicle_id: str) -> ControllerState: """Retrieve the controller state of the given vehicle.""" @@ -802,7 +782,7 @@ def controller_state_for_vehicle_id(self, vehicle_id: str) -> ControllerState: return self._controller_states[vehicle_id] def load_controller_params(self, controller_filepath: str): - """Set the default controller parameters for actor controlled vehicles.""" + """Set the default controller parameters for owner controlled vehicles.""" self._controller_params = resources.load_controller_params(controller_filepath) def controller_params_for_vehicle_type(self, vehicle_type: str): @@ -815,13 +795,13 @@ def _build_empty_controlled_by(): return np.array( [], dtype=[ - # E.g. [(, , ), ...] + # E.g. [(, , ), ...] ("vehicle_id", f"|S{VEHICLE_INDEX_ID_LENGTH}"), - ("actor_id", f"|S{VEHICLE_INDEX_ID_LENGTH}"), - ("actor_role", "B"), - # XXX: Keeping things simple, this is always assumed to be an agent. - # We can add an shadow_actor_role when needed - ("shadow_actor_id", f"|S{VEHICLE_INDEX_ID_LENGTH}"), + ("owner_id", f"|S{VEHICLE_INDEX_ID_LENGTH}"), + ("role", "B"), + # XXX: Keeping things simple, this is currently assumed to be an agent. + # We can add a shadower_role when needed + ("shadower_id", f"|S{VEHICLE_INDEX_ID_LENGTH}"), ("is_boid", "B"), ("is_hijacked", "B"), ("position", np.float64, (3,)), @@ -837,12 +817,12 @@ def __repr__(self): ) by["position"] = [", ".join([f"{x:.2f}" for x in p]) for p in by["position"]] - by["actor_id"] = [str(truncate(p, 20)) for p in by["actor_id"]] + by["owner_id"] = [str(truncate(p, 20)) for p in by["owner_id"]] by["vehicle_id"] = [str(truncate(p, 20)) for p in by["vehicle_id"]] - by["shadow_actor_id"] = [str(truncate(p, 20)) for p in by["shadow_actor_id"]] + by["shadower_id"] = [str(truncate(p, 20)) for p in by["shadower_id"]] by["is_boid"] = [str(bool(x)) for x in by["is_boid"]] by["is_hijacked"] = [str(bool(x)) for x in by["is_hijacked"]] - by["actor_role"] = [str(ActorRole(x)).split(".")[-1] for x in by["actor_role"]] + by["role"] = [str(ActorRole(x)).split(".")[-1] for x in by["role"]] # XXX: tableprint crashes when there's no data if by.size == 0: diff --git a/smarts/core/vehicle_state.py b/smarts/core/vehicle_state.py new file mode 100644 index 0000000000..cee9b8c10c --- /dev/null +++ b/smarts/core/vehicle_state.py @@ -0,0 +1,185 @@ +# MIT License +# +# Copyright (C) 2022. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from dataclasses import dataclass +from typing import NamedTuple, Optional, Tuple + +import numpy as np +from scipy.spatial.distance import cdist +from shapely.affinity import rotate as shapely_rotate +from shapely.geometry import Polygon +from shapely.geometry import box as shapely_box + +from .actor import ActorState +from .colors import SceneColors +from .coordinates import Dimensions, Heading, Pose +from .utils.math import rotate_cw_around_point + + +@dataclass(frozen=True) +class VehicleConfig: + """Vehicle configuration""" + + vehicle_type: str + color: SceneColors + dimensions: Dimensions + glb_model: str + + +# A mapping between SUMO's vehicle types and our internal vehicle config. +# TODO: Don't leak SUMO's types here. +# XXX: The GLB's dimensions must match the specified dimensions here. +# TODO: for traffic histories, vehicle (and road) dimensions are set by the dataset +VEHICLE_CONFIGS = { + "passenger": VehicleConfig( + vehicle_type="car", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=3.68, width=1.47, height=1.4), + glb_model="simple_car.glb", + ), + "bus": VehicleConfig( + vehicle_type="bus", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=7, width=2.25, height=3), + glb_model="bus.glb", + ), + "coach": VehicleConfig( + vehicle_type="coach", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=8, width=2.4, height=3.5), + glb_model="coach.glb", + ), + "truck": VehicleConfig( + vehicle_type="truck", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=5, width=1.91, height=1.89), + glb_model="truck.glb", + ), + "trailer": VehicleConfig( + vehicle_type="trailer", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=10, width=2.5, height=4), + glb_model="trailer.glb", + ), + "pedestrian": VehicleConfig( + vehicle_type="pedestrian", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=0.5, width=0.5, height=1.6), + glb_model="pedestrian.glb", + ), + "motorcycle": VehicleConfig( + vehicle_type="motorcycle", + color=SceneColors.SocialVehicle, + dimensions=Dimensions(length=2.5, width=1, height=1.4), + glb_model="motorcycle.glb", + ), +} + + +class Collision(NamedTuple): + """Represents a collision by an ego vehicle with another vehicle.""" + + # XXX: This might not work for boid agents + collidee_id: str + + +@dataclass +class VehicleState(ActorState): + """Vehicle state information.""" + + vehicle_config_type: Optional[str] = None # key into VEHICLE_CONFIGS + pose: Optional[Pose] = None + dimensions: Optional[Dimensions] = None + speed: float = 0.0 + steering: Optional[float] = None + yaw_rate: Optional[float] = None + linear_velocity: Optional[np.ndarray] = None + angular_velocity: Optional[np.ndarray] = None + linear_acceleration: Optional[np.ndarray] = None + angular_acceleration: Optional[np.ndarray] = None + + def __post_init__(self): + assert self.pose is not None and self.dimensions is not None + + def __eq__(self, __o: object): + return super().__eq__(__o) + + @property + def bounding_box_points( + self, + ) -> Tuple[ + Tuple[float, float], + Tuple[float, float], + Tuple[float, float], + Tuple[float, float], + ]: + """The minimum fitting heading aligned bounding box. Four 2D points representing the minimum fitting box.""" + # Assuming the position is the centre, + # calculate the corner coordinates of the bounding_box + origin = self.pose.position[:2] + dimensions = np.array([self.dimensions.width, self.dimensions.length]) + corners = np.array([(-1, 1), (1, 1), (1, -1), (-1, -1)]) / 2 + heading = self.pose.heading + return tuple( + tuple( + rotate_cw_around_point( + point=origin + corner * dimensions, + radians=Heading.flip_clockwise(heading), + origin=origin, + ) + ) + for corner in corners + ) + + @property + def bbox(self) -> Polygon: + """Returns a bounding box around the vehicle.""" + pos = self.pose.point + half_len = 0.5 * self.dimensions.length + half_width = 0.5 * self.dimensions.width + poly = shapely_box( + pos.x - half_width, + pos.y - half_len, + pos.x + half_width, + pos.y + half_len, + ) + return shapely_rotate(poly, self.pose.heading, use_radians=True) + + +def neighborhood_vehicles_around_vehicle( + vehicle_state, vehicle_states, radius: Optional[float] = None +): + """Determines what vehicles are within the radius (if given).""" + other_states = [v for v in vehicle_states if v.actor_id != vehicle_state.actor_id] + if radius is None: + return other_states + + other_positions = [state.pose.position for state in other_states] + if not other_positions: + return [] + + # calculate euclidean distances + distances = cdist( + other_positions, [vehicle_state.pose.position], metric="euclidean" + ).reshape(-1) + + indices = np.argwhere(distances <= radius).flatten() + return [other_states[i] for i in indices] diff --git a/smarts/engine.ini b/smarts/engine.ini index eec1cf05c5..e65909b197 100644 --- a/smarts/engine.ini +++ b/smarts/engine.ini @@ -1,8 +1,13 @@ [benchmark] [core] +debug = false +observation_workers = 0 +reset_retries = 0 [controllers] [physics] max_pybullet_freq = 240 [providers] [rendering] -[resources] \ No newline at end of file +[resources] +[ray] +log_to_driver=False \ No newline at end of file diff --git a/smarts/env/tests/test_determinism.py b/smarts/env/tests/test_determinism.py index 6f3bf80250..63abb8d50a 100644 --- a/smarts/env/tests/test_determinism.py +++ b/smarts/env/tests/test_determinism.py @@ -33,6 +33,7 @@ from smarts.core.agent_interface import AgentInterface from smarts.core.controllers import ActionSpaceType from smarts.core.utils.episodes import episodes +from smarts.core.utils.logging import diff_unpackable from smarts.zoo.agent_spec import AgentSpec @@ -94,14 +95,6 @@ def vehicle_state_check(vs_now, vs_prev): assert vs_now.lane_index == vs_prev.lane_index -def axis_dive(now_axis, prev_axis): - for (now, prev) in zip(now_axis, prev_axis): - if isinstance(now, np.ndarray) or isinstance(now, Sequence): - axis_dive(now, prev) - else: - assert now == prev - - def determinism(agent_spec, scenarios, episode_count, capture_step): rewards_capture = [] dones_capture = [] @@ -121,77 +114,7 @@ def check_callback(rewards, agent_obs, dones, index): assert len(dones) == len(dones_capture[index]) assert all([ds == ds2 for (ds, ds2) in zip(dones, dones_capture[index])]) - # "events", - events_now = agent_obs.events - events_prev = orig_agent_obs.events - - assert len(events_now.collisions) == len(events_prev.collisions) - assert all( - [ - now == prev - for (now, prev) in zip(events_now.collisions, events_prev.collisions) - ] - ) - assert events_now.off_road == events_prev.off_road - assert events_now.reached_goal == events_prev.reached_goal - assert ( - events_now.reached_max_episode_steps - == events_prev.reached_max_episode_steps - ) - - # "ego_vehicle_state", - evs_now = agent_obs.ego_vehicle_state - evs_prev = orig_agent_obs.ego_vehicle_state - vehicle_state_check(evs_now, evs_prev) - - # "neighborhood_vehicle_states", - nbvs_now = agent_obs.neighborhood_vehicle_states - nbvs_prev = orig_agent_obs.neighborhood_vehicle_states - - assert len(nbvs_now) == len(nbvs_prev) - for now, prev in zip(nbvs_now, nbvs_prev): - vehicle_state_check(now, prev) - - # "waypoint_paths", - assert len(agent_obs.waypoint_paths) == len(orig_agent_obs.waypoint_paths) - - for w_paths_now, w_paths_prev in zip( - agent_obs.waypoint_paths, orig_agent_obs.waypoint_paths - ): - for now, prev in zip(w_paths_now, w_paths_prev): - assert now == prev - # np.array is requiring this - assert all( - [ - pos_now == pos_prev - for (pos_now, pos_prev) in zip(now.pos, prev.pos) - ] - ) - assert now.heading == prev.heading - assert now.lane_width == prev.lane_width - assert now.speed_limit == prev.speed_limit - assert now.lane_id == prev.lane_id - assert now.lane_index == prev.lane_index - - # "distance_travelled", - assert agent_obs.distance_travelled == orig_agent_obs.distance_travelled - - # "lidar_point_cloud", - axis_dive(agent_obs.lidar_point_cloud, orig_agent_obs.lidar_point_cloud) - - # "drivable_area_grid_map", - axis_dive( - agent_obs.drivable_area_grid_map.data, - orig_agent_obs.drivable_area_grid_map.data, - ) - - # "occupancy_grid_map", - axis_dive( - agent_obs.occupancy_grid_map.data, orig_agent_obs.occupancy_grid_map.data - ) - - # "top_down_rgb", - axis_dive(agent_obs.top_down_rgb.data, orig_agent_obs.top_down_rgb.data) + assert diff_unpackable(agent_obs, orig_agent_obs) == "" run(agent_spec, capture_callback, scenarios, episode_count, capture_step) run(agent_spec, check_callback, scenarios, episode_count, capture_step) diff --git a/smarts/env/tests/test_shutdown.py b/smarts/env/tests/test_shutdown.py index 2ba5acff3b..39b27be4b3 100644 --- a/smarts/env/tests/test_shutdown.py +++ b/smarts/env/tests/test_shutdown.py @@ -76,7 +76,8 @@ def test_graceful_interrupt(monkeypatch): # To simulate a user interrupting the sim (e.g. ctrl-c). We just need to # hook in to some function that SMARTS calls internally (like this one). with mock.patch( - "smarts.core.sensors.Sensors.observe", side_effect=KeyboardInterrupt + "smarts.core.sensor_manager.SensorManager.observe", + side_effect=KeyboardInterrupt, ): for episode in range(10): obs, _, _, _ = env.step({AGENT_ID: agent.act(obs)}) diff --git a/smarts/env/utils/observation_conversion.py b/smarts/env/utils/observation_conversion.py index c03ea862fb..3b103a1ea4 100644 --- a/smarts/env/utils/observation_conversion.py +++ b/smarts/env/utils/observation_conversion.py @@ -184,7 +184,6 @@ def _format_signals(signals: List[SignalObservation]): def _format_neighborhood_vehicle_states( neighborhood_vehicle_states: List[VehicleObservation], ): - ## TODO MTA: Add in the vehicle ids des_shp = _NEIGHBOR_SHP rcv_shp = len(neighborhood_vehicle_states) pad_shp = 0 if des_shp - rcv_shp < 0 else des_shp - rcv_shp diff --git a/smarts/ray/__init__.py b/smarts/ray/__init__.py new file mode 100644 index 0000000000..2c65923417 --- /dev/null +++ b/smarts/ray/__init__.py @@ -0,0 +1,21 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/smarts/ray/sensors/__init__.py b/smarts/ray/sensors/__init__.py new file mode 100644 index 0000000000..2c65923417 --- /dev/null +++ b/smarts/ray/sensors/__init__.py @@ -0,0 +1,21 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/smarts/ray/sensors/ray_sensor_resolver.py b/smarts/ray/sensors/ray_sensor_resolver.py new file mode 100644 index 0000000000..f59e962bd4 --- /dev/null +++ b/smarts/ray/sensors/ray_sensor_resolver.py @@ -0,0 +1,197 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import concurrent.futures +import logging +from collections import defaultdict +from typing import Optional, Set + +import ray + +from smarts.core import config +from smarts.core.configuration import Config +from smarts.core.sensors import SensorResolver, Sensors +from smarts.core.serialization.default import dumps, loads +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.utils.file import replace +from smarts.core.utils.logging import timeit + +logger = logging.getLogger(__name__) + + +class RaySensorResolver(SensorResolver): + """A version of the sensor resolver that uses "ray" in its underlying implementation. + + Args: + process_count_override (Optional[int]): An override for how many workers should be used. + """ + + def __init__(self, process_count_override: Optional[int] = None) -> None: + conf: Config = config() + self._num_observation_workers = ( + conf("core", "observation_workers", default=8, cast=int) + if process_count_override == None + else max(1, process_count_override) + ) + if not ray.is_initialized(): + ray.init( + num_cpus=self._num_observation_workers, + log_to_driver=conf("ray", "log_to_driver", default=False, cast=bool), + ) + self._sim_local_constants: SimulationLocalConstants = None + self._current_workers = [] + + def get_ray_worker_actors(self, count: int): + """Get the current "ray" worker actors. + + Args: + count (int): The number of workers to get. + + Returns: + Any: The "ray" remote worker handles. + """ + if len(self._current_workers) != count: + # we need to cache because using options(name) is extremely slow + self._current_workers = [ + RayProcessWorker.options( + name=f"sensor_worker_{i}", get_if_exists=True + ).remote() + for i in range(count) + ] + return self._current_workers + + def observe( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], + renderer, + bullet_client, + ): + observations, dones, updated_sensors = {}, {}, defaultdict(dict) + + ray_actors = self.get_ray_worker_actors(self._num_observation_workers) + len_workers = len(ray_actors) + + tasks = [] + with timeit( + f"parallizable observations with {len(agent_ids)} and {len(ray_actors)}", + logger.info, + ): + # Update remote state (if necessary) + remote_sim_frame = ray.put(dumps(sim_frame)) + if self._sim_local_constants is None or ( + self._sim_local_constants.road_map_hash + != sim_local_constants.road_map_hash + ): + remote_sim_local_constants = ray.put(dumps(sim_local_constants)) + for a in ray_actors: + a.update_local_constants.remote(remote_sim_local_constants) + + # Start remote tasks + agent_ids_for_grouping = list(agent_ids) + agent_groups = [ + frozenset(agent_ids_for_grouping[i::len_workers]) + for i in range(len_workers) + ] + for i, agent_group in enumerate(agent_groups): + if not agent_group: + break + with timeit(f"submitting {len(agent_group)} agents", logger.info): + tasks.append( + ray_actors[i].do_work.remote( + remote_sim_frame=remote_sim_frame, agent_ids=agent_group + ) + ) + + # While observation processes are operating do rendering + with timeit("rendering", logger.info): + rendering = {} + for agent_id in agent_ids: + for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: + ( + rendering[agent_id], + updated_unsafe_sensors, + ) = Sensors.process_serialization_unsafe_sensors( + sim_frame, + sim_local_constants, + agent_id, + sim_frame.sensor_states[vehicle_id], + vehicle_id, + renderer, + bullet_client, + ) + updated_sensors[vehicle_id].update(updated_unsafe_sensors) + + # Collect futures + with timeit("waiting for observations", logger.info): + for fut in concurrent.futures.as_completed( + [task.future() for task in tasks] + ): + obs, ds, u_sens = fut.result() + observations.update(obs) + dones.update(ds) + for v_id, values in u_sens.items(): + updated_sensors[v_id].update(values) + + with timeit("merging observations", logger.info): + # Merge sensor information + for agent_id, r_obs in rendering.items(): + observations[agent_id] = replace(observations[agent_id], **r_obs) + + return observations, dones, updated_sensors + + def step(self, sim_frame, sensor_states): + """Step the sensor state.""" + for sensor_state in sensor_states: + sensor_state.step() + + +@ray.remote +class RayProcessWorker: + """A `ray` based process worker for parallel operation on sensors.""" + + def __init__(self) -> None: + self._simulation_local_constants: Optional[SimulationLocalConstants] = None + + def update_local_constants(self, sim_local_constants): + """Updates the process worker. + + Args: + sim_local_constants (SimulationLocalConstants | None): The current simulation reset state. + """ + self._simulation_local_constants = loads(sim_local_constants) + + def do_work(self, remote_sim_frame, agent_ids): + """Run the sensors against the current simulation state. + + Args: + remote_sim_frame (SimulationFrame): The current simulation state. + agent_ids (set[str]): The agent ids to operate on. + + Returns: + tuple[dict, dict, dict]: The updated sensor states: (observations, dones, updated_sensors) + """ + sim_frame = loads(remote_sim_frame) + return Sensors.observe_serializable_sensor_batch( + sim_frame, self._simulation_local_constants, agent_ids + ) diff --git a/smarts/ray/sensors/tests/test_ray_sensor_resolver.py b/smarts/ray/sensors/tests/test_ray_sensor_resolver.py new file mode 100644 index 0000000000..5a7373c67c --- /dev/null +++ b/smarts/ray/sensors/tests/test_ray_sensor_resolver.py @@ -0,0 +1,134 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import pytest + +from smarts.core.agent_interface import AgentInterface, AgentType +from smarts.core.controllers.action_space_type import ActionSpaceType +from smarts.core.plan import Mission +from smarts.core.scenario import Scenario +from smarts.core.sensors.local_sensor_resolver import LocalSensorResolver +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.smarts import SMARTS +from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation +from smarts.core.utils.logging import diff_unpackable +from smarts.ray.sensors.ray_sensor_resolver import RaySensorResolver + +AGENT_IDS = [f"agent-00{i}" for i in range(3)] + + +@pytest.fixture +def scenario() -> Scenario: + s = Scenario( + scenario_root="scenarios/sumo/loop", + traffic_specs=["scenarios/sumo/loop/build/traffic/basic.rou.xml"], + missions=dict( + zip( + AGENT_IDS, + Scenario.discover_agent_missions( + scenario_root="scenarios/sumo/loop", + agents_to_be_briefed=AGENT_IDS, + ), + ) + ), + ) + missions = [ + Mission.random_endless_mission( + s.road_map, + ) + for _ in AGENT_IDS + ] + s.set_ego_missions(dict(zip(AGENT_IDS, missions))) + return s + + +@pytest.fixture( + params=[ + AgentInterface.from_type( + AgentType.Laner, + action=ActionSpaceType.Continuous, + ), + AgentInterface.from_type( + AgentType.Full, + action=ActionSpaceType.Continuous, + ), + ] +) +def sim(scenario, request): + a_interface = getattr(request, "param") + agents = {aid: a_interface for aid in AGENT_IDS} + smarts = SMARTS( + agents, + traffic_sims=[SumoTrafficSimulation()], + envision=None, + ) + smarts.reset(scenario) + smarts.step({aid: [0, 0, 0] for aid in AGENT_IDS}) + yield smarts + smarts.destroy() + + +def test_sensor_parallelization( + sim: SMARTS, +): + del sim.cached_frame + simulation_frame: SimulationFrame = sim.cached_frame + simulation_local_constants: SimulationLocalConstants = sim.local_constants + + parallel_resolver = RaySensorResolver(process_count_override=1) + serial_resolver = LocalSensorResolver() + + parallel_resolver.get_ray_worker_actors(1) + + assert len(simulation_frame.agent_ids) > 1 + p_observations, p_dones, p_updated_sensors = parallel_resolver.observe( + sim_frame=simulation_frame, + sim_local_constants=simulation_local_constants, + agent_ids=simulation_frame.agent_ids, + renderer=sim.renderer, + bullet_client=sim.bc, + ) + + l_observations, l_dones, l_updated_sensors = serial_resolver.observe( + sim_frame=simulation_frame, + sim_local_constants=simulation_local_constants, + agent_ids=simulation_frame.agent_ids, + renderer=sim.renderer, + bullet_client=sim.bc, + ) + + assert len(p_observations) > 0 + assert diff_unpackable(p_observations, l_observations) == "" + + assert len(p_dones) > 0 + assert diff_unpackable(p_dones, l_dones) == "" + + assert len(p_updated_sensors) > 0 + assert p_updated_sensors.keys() == l_updated_sensors.keys() + assert set(p_updated_sensors.keys()) not in simulation_frame.agent_ids + + # TODO: Make sure that all mutable sensors are returned + for agent_id, p_sensors in p_updated_sensors.items(): + assert len(p_sensors) > 0 + assert p_sensors.keys() == l_updated_sensors[agent_id].keys() + for k in p_sensors: + assert p_sensors[k] == l_updated_sensors[agent_id][k]