Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix observe_from(). #2134

Merged
merged 6 commits into from
Jan 7, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,16 @@ Copy and pasting the git commit messages is __NOT__ enough.

## [Unreleased] - XXXX-XX-XX
### Added
- Added a utility method `SMARTS.prepare_observe_from()` which allows safely adding sensors to vehicles.
- The following methods now exist explicitly `Vehicle.{add_sensor|detach_sensor|subscribed_to|sensor_property|}`.
### Changed
- `VehicleIndex.build_agent_vehicle()` no longer has `filename` and `surface_patches` parameters.
- The following modules have been renamed: `envision.types` -> `envision.etypes`, `smarts.core.utils.logging` -> `smarts.core.utils.core_logging`, `smarts.core.utils.math` -> `smarts.core.utils.core_math`, `smarts.sstudio.types` -> `smarts.sstudio.sstypes`. For compatibility reasons they can still be imported by their original module name.
- Exposed `traffic:traci_retries`/`SMARTS_TRAFFIC_TRACI_RETRIES` to control how many times the `SumoTrafficSimulation` will try to restart when using default configuration.
- `rllib` is now constrained as `<=2.9,>2.4`.
- The `examples/e12_rllib` training examples `{pg_example|pg_pbt_example}.py` have been changed to `{ppo_example|ppo_pbt_example}.py`. `Policy Gradients (PG)` has been dropped in favor of the more well documented `Proximal Policy Optimization (PPO)`.
- Vehicles can now have sensors added to, overwritten, or replaced outright.
- Logging is now improved to give information about sensor changes in the sensor manager.
### Deprecated
### Fixed
- `SumoTrafficSimulation` gives clearer reasons as to why it failed to connect to the TraCI server.
Expand All @@ -34,6 +38,10 @@ Copy and pasting the git commit messages is __NOT__ enough.
- Fixed an issue where you would need to install `waymo` in order to use any kind of dataset histories.
- Fixed an issue where Pycharm would load `smarts/sstudio/types` as the integrated `types` module. See #2125.
- Fixed an issue where the `e12_rllib` examples would use the wrong path for the default loop scenario.
- Fixed an issue where the sensor state could be `None` when calling `SMARTS.observe_from()` on a non-ego vehicle. See #2133.
- The via sensor and trip meter sensor now work without a mission.
- Fixed a bug with `VehicleIndex.attach_sensors_to_vehicle()` that would generate an invalid plan.
- Fixed a bug where vehicle sensor meta attributes would reference the wrong vehicle.
### Removed
### Security

Expand Down
6 changes: 2 additions & 4 deletions smarts/core/agent_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -268,12 +268,10 @@ class AgentInterface:
debug: bool = False
"""Enable debug information for the various sensors and action spaces."""

event_configuration: EventConfiguration = field(
default_factory=lambda: EventConfiguration()
)
event_configuration: EventConfiguration = field(default_factory=EventConfiguration)
"""Configurable criteria of when to trigger events"""

done_criteria: DoneCriteria = field(default_factory=lambda: DoneCriteria())
done_criteria: DoneCriteria = field(default_factory=DoneCriteria)
"""Configurable criteria of when to mark this actor as done. Done actors will be
removed from the environment and may trigger the episode to be done."""

Expand Down
7 changes: 6 additions & 1 deletion smarts/core/plan.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@
import sys
import warnings
from dataclasses import dataclass, field
from typing import List, Optional, Tuple
from typing import List, Optional, Tuple, Type

import numpy as np

Expand Down Expand Up @@ -301,6 +301,11 @@ class PlanFrame:
road_ids: List[str]
mission: Optional[Mission]

@classmethod
def empty(cls: Type[PlanFrame]):
"""Creates an empty plan frame."""
return cls(road_ids=[], mission=None)


class Plan:
"""Describes a navigation plan (route) to fulfill a mission."""
Expand Down
6 changes: 5 additions & 1 deletion smarts/core/sensor.py
Original file line number Diff line number Diff line change
Expand Up @@ -394,8 +394,9 @@ def update_distance_wps_record(
wp_road = road_map.lane_by_id(new_wp.lane_id).road.road_id

should_count_wp = (
plan.mission == None
# if we do not have a fixed route, we count all waypoints we accumulate
not plan.mission.requires_route
or 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]
)
Expand Down Expand Up @@ -664,6 +665,9 @@ def __eq__(self, __value: object) -> bool:
def __call__(self, vehicle_state: VehicleState, plan, road_map):
near_points: List[ViaPoint] = list()
hit_points: List[ViaPoint] = list()
if plan.mission is None:
return (near_points, hit_points)

vehicle_position = vehicle_state.pose.position[:2]

@lru_cache()
Expand Down
34 changes: 23 additions & 11 deletions smarts/core/sensor_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,12 @@
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._logger = logging.getLogger(self.__class__.__name__)
self._sensors: Dict[str, Sensor] = {}

# {actor_id: <SensorState>}
Expand All @@ -55,7 +54,7 @@ def __init__(self):
self._actors_by_sensor_id: Dict[str, Set[str]] = {}
self._sensor_references = Counter()
# {sensor_id, ...}
self._scheduled_sensors: List[Sensor] = []
self._scheduled_sensors: List[Tuple[str, Sensor]] = []
observation_workers = config()(
"core", "observation_workers", default=0, cast=int
)
Expand Down Expand Up @@ -168,6 +167,7 @@ def observe_batch(

def teardown(self, renderer):
"""Tear down the current sensors and clean up any internal resources."""
self._logger.info("++ Sensors and sensor states reset. ++")
for sensor in self._sensors.values():
sensor.teardown(renderer=renderer)
self._sensors = {}
Expand All @@ -178,23 +178,31 @@ def teardown(self, renderer):

def add_sensor_state(self, actor_id: str, sensor_state: SensorState):
"""Add a sensor state associated with a given actor."""
self._logger.debug("Sensor state added for actor '%s'.", actor_id)
self._sensor_states[actor_id] = sensor_state

def remove_sensors_by_actor_id(
def remove_sensor_state_by_actor_id(self, actor_id: str):
"""Add a sensor state associated with a given actor."""
self._logger.debug("Sensor state removed for actor '%s'.", actor_id)
return self._sensor_states.pop(actor_id, None)

def remove_actor_sensors_by_actor_id(
self, actor_id: str, schedule_teardown: bool = True
) -> Iterable[Tuple[Sensor, int]]:
"""Remove association of an actor to sensors. If the sensor is no longer associated an actor, 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
self._logger.warning(
"Attempted to remove sensors from actor with no sensors: '%s'.",
actor_id,
)
return []
del self._sensor_states[actor_id]
self.remove_sensor_state_by_actor_id(actor_id)
sensors_by_actor = self._sensors_by_actor_id.get(actor_id)
if not sensors_by_actor:
return []
self._logger.debug("Target sensor removal for actor '%s'.", actor_id)
discarded_sensors = []
for sensor_id in sensors_by_actor:
self._actors_by_sensor_id[sensor_id].remove(actor_id)
Expand All @@ -210,6 +218,7 @@ def remove_sensor(
self, sensor_id: str, schedule_teardown: bool = False
) -> Optional[Sensor]:
"""Remove a sensor by its id. Removes any associations it has with actors."""
self._logger.debug("Target removal of sensor '%s'.", sensor_id)
sensor = self._sensors.get(sensor_id)
if not sensor:
return None
Expand All @@ -218,8 +227,9 @@ def remove_sensor(

def _disassociate_sensor(self, sensor_id, schedule_teardown):
if schedule_teardown:
self._scheduled_sensors.append(self._sensors[sensor_id])
self._scheduled_sensors.append((sensor_id, self._sensors[sensor_id]))

self._logger.info("Sensor '%s' removed from manager.", sensor_id)
del self._sensors[sensor_id]
del self._sensor_references[sensor_id]

Expand Down Expand Up @@ -275,7 +285,7 @@ def add_sensor_for_actor(self, actor_id: str, name: str, sensor: Sensor) -> str:
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(
self._logger.warning(
"Duplicate sensor attempted to add to actor `%s`: `%s`", actor_id, s_id
)
return s_id
Expand All @@ -286,6 +296,7 @@ def add_sensor_for_actor(self, actor_id: str, name: str, sensor: Sensor) -> str:

def add_sensor(self, sensor_id, sensor: Sensor) -> str:
"""Adds a sensor to the sensor manager."""
self._logger.info("Added sensor '%s' to sensor manager.", sensor_id)
assert sensor_id not in self._sensors
self._sensors[sensor_id] = sensor
self._sensor_references.update([sensor_id])
Expand All @@ -298,9 +309,10 @@ def clean_up_sensors_for_actors(self, current_actor_ids: Set[str], renderer):
missing_actors = old_actor_ids - current_actor_ids

for aid in missing_actors:
self.remove_sensors_by_actor_id(aid)
self.remove_actor_sensors_by_actor_id(aid)

for sensor in self._scheduled_sensors:
for sensor_id, sensor in self._scheduled_sensors:
self._logger.info("Sensor '%s' destroyed.", sensor_id)
sensor.teardown(renderer=renderer)

self._scheduled_sensors.clear()
15 changes: 11 additions & 4 deletions smarts/core/sensors/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,13 @@
# 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 __future__ import annotations

import logging
import math
import re
import sys
from typing import Any, Dict, List, Optional, Sequence, Set, Tuple
from typing import Any, Dict, List, Optional, Sequence, Set, Tuple, Type

import numpy as np

Expand Down Expand Up @@ -111,7 +113,7 @@ def _make_vehicle_observation(
class SensorState:
"""Sensor state information"""

def __init__(self, max_episode_steps: int, plan_frame: PlanFrame):
def __init__(self, max_episode_steps: Optional[int], plan_frame: PlanFrame):
self._max_episode_steps = max_episode_steps
self._plan_frame = plan_frame
self._step = 0
Expand Down Expand Up @@ -157,6 +159,11 @@ def steps_completed(self) -> int:
"""Get the number of steps where this sensor has been updated."""
return self._step

@classmethod
def invalid(cls: Type[SensorState]) -> SensorState:
"""Generate a invalid default frame."""
return cls(None, PlanFrame.empty())


class SensorResolver:
"""An interface describing sensor observation and update systems."""
Expand Down Expand Up @@ -272,7 +279,7 @@ def process_serialization_safe_sensors(
sim_frame: SimulationFrame,
sim_local_constants: SimulationLocalConstants,
interface: AgentInterface,
sensor_state,
sensor_state: SensorState,
vehicle_id,
agent_id=None,
):
Expand Down Expand Up @@ -652,7 +659,7 @@ def _is_done_with_events(
def _agent_reached_goal(
cls, sensor_state, plan, vehicle_state: VehicleState, trip_meter_sensor
):
if not trip_meter_sensor:
if not trip_meter_sensor or plan.mission is None:
return False
distance_travelled = trip_meter_sensor()
mission = plan.mission
Expand Down
63 changes: 56 additions & 7 deletions smarts/core/smarts.py
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
from smarts.core.id_actor_capture_manager import IdActorCaptureManager
from smarts.core.plan import Plan
from smarts.core.renderer_base import RendererBase
from smarts.core.sensors import SensorState
from smarts.core.simulation_local_constants import SimulationLocalConstants
from smarts.core.utils.core_logging import timeit
from smarts.core.utils.type_operations import TypeSuite
Expand Down Expand Up @@ -755,7 +756,7 @@ def vehicle_exited_bubble(
if teardown_agent:
self.teardown_social_agents([shadow_agent_id])
if self._vehicle_index.shadower_id_from_vehicle_id(vehicle_id) is None:
self._sensor_manager.remove_sensors_by_actor_id(vehicle_id)
self._sensor_manager.remove_actor_sensors_by_actor_id(vehicle_id)

def _agent_releases_actor(
self,
Expand Down Expand Up @@ -951,28 +952,76 @@ def _teardown_vehicles(self, vehicle_ids):
for v_id in vehicle_ids:
self._remove_vehicle_from_providers(v_id)

def attach_sensors_to_vehicles(self, agent_interface, vehicle_ids):
def attach_sensors_to_vehicles(
self,
agent_interface: AgentInterface,
vehicle_ids,
overwrite_sensors=False,
reset_sensors=False,
):
"""Set the specified vehicles with the sensors needed to satisfy the specified agent
interface.
interface. See :attr:`smarts.core.smarts.SMARTS.prepare_observe_from`.

Args:
agent_interface (AgentInterface): The minimum interface generating the observations.
vehicle_ids (Sequence[str]): The vehicles to target.
overwrite_sensors (bool, optional): If to replace existing sensors (USE CAREFULLY). Defaults to False.
reset_sensors (bool, optional): If to remove all existing sensors before adding sensors (USE **VERY** CAREFULLY). Defaults to False.
"""
self.prepare_observe_from(
vehicle_ids, agent_interface, overwrite_sensors, reset_sensors
)

def prepare_observe_from(
self,
vehicle_ids: Sequence[str],
interface: AgentInterface,
overwrite_sensors,
reset_sensors,
):
"""Assigns the given vehicle sensors as is described by the agent interface.

Args:
vehicle_ids (Sequence[str]): The vehicles to target.
interface (AgentInterface): The minimum interface generating the observations.
overwrite_sensors (bool, optional): If to replace existing sensors (USE CAREFULLY).
reset_sensors (bool, optional): If to remove all existing sensors (USE **VERY** CAREFULLY).
"""
self._check_valid()

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
self.sensor_manager,
self,
v,
interface,
replace=overwrite_sensors,
reset_sensors=reset_sensors,
)

def observe_from(
self, vehicle_ids: Sequence[str], interface: AgentInterface
self,
vehicle_ids: Sequence[str],
interface: AgentInterface,
) -> Tuple[Dict[str, Observation], Dict[str, bool]]:
"""Generate observations from the specified vehicles."""
"""Generate observations from the specified vehicles.

Args:
vehicle_ids (Sequence[str]): The vehicles to target.
interface (AgentInterface): The intended interface generating the observations (this may be ignored.)

Returns:
Tuple[Dict[str, Observation], Dict[str, bool]]: A dictionary of observations and the hypothetical dones.
"""
self._check_valid()

vehicles = {
vehicles: Dict[str, Vehicle] = {
v_id: self.vehicle_index.vehicle_by_id(v_id) for v_id in vehicle_ids
}
sensor_states = {
vehicle.id: self._sensor_manager.sensor_state_for_actor_id(vehicle.id)
or SensorState.invalid()
for vehicle in vehicles.values()
}
return self.sensor_manager.observe_batch(
Expand Down
Loading
Loading