Fix observe_from().

CHANGELOG.md

@@ -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.
@@ -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
 

smarts/core/agent_interface.py

@@ -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."""
 

smarts/core/plan.py

@@ -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
 
@@ -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."""

smarts/core/sensor.py

@@ -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]
         )
@@ -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()

smarts/core/sensor_manager.py

@@ -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>}
@@ -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
         )
@@ -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 = {}
@@ -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)
@@ -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
@@ -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]
 
@@ -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
@@ -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])
@@ -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()

smarts/core/sensors/__init__.py

@@ -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
 
@@ -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
@@ -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."""
@@ -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,
     ):
@@ -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

smarts/core/smarts.py

@@ -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
@@ -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,
@@ -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(