Fix duplicate sensor warning (#2041)

* Fix duplicate sensor warning.

* Fix sensor comparison.

* Update changelog.

* Remove sensor instantiation redundancy.

CHANGELOG.md

@@ -42,6 +42,8 @@ Copy and pasting the git commit messages is __NOT__ enough.
 - `SumoTrafficSimulation` no longer reports that it manages vehicles when it is disconnected.
 - Fixed waypoints so that they will always be the length of the `lookahead` parameter, even in junctions.
 - Fixed an issue where a single waypoint would appear in off-route lanes for missions with a goal.
+- Fixed an underlying issue with the sensor manager where the sensors were not removed immediately.
+- Fixed an issue where warnings could be generated when an agent takes over an existing vehicle if the vehicle previously had sensors on it.
 ### Removed
 - Removed the following dependencies from smarts: `pandas`, `rich`, `twisted`, `sh`.
 - Moved `baselines/marl_benchmark` from this repository to `smarts-project/smarts-project.rl` repository.

scenarios/sumo/intersections/6lane/scenario.py

@@ -2,7 +2,10 @@
 from pathlib import Path
 
 from smarts.sstudio import gen_scenario
-from smarts.sstudio.types import Mission, Route, Scenario, SocialAgentActor
+from smarts.sstudio.types import Mission, Route, Scenario, SocialAgentActor, Traffic
+from smarts.sstudio.types.entry_tactic import IdEntryTactic
+from smarts.sstudio.types.route import RandomRoute
+from smarts.sstudio.types.traffic import Trip
 
 
 def actor_gen(id_):
@@ -23,12 +26,33 @@ def to_mission(start_edge, end_edge):
 
 gen_scenario(
     Scenario(
+        ego_missions=[
+            Mission(
+                Route(begin=("edge-north-NS", 0, 20), end=("edge-south-NS", 0, "max")),
+                entry_tactic=IdEntryTactic(2, "other_interest"),
+            )
+        ],
         social_agent_missions={
             "group-1": (actor_gen(1), [to_mission("edge-north-NS", "edge-south-NS")]),
             "group-2": (actor_gen(2), [to_mission("edge-west-WE", "edge-east-WE")]),
             "group-3": (actor_gen(3), [to_mission("edge-east-EW", "edge-west-EW")]),
             "group-4": (actor_gen(4), [to_mission("edge-south-SN", "edge-north-SN")]),
-        }
+        },
+        traffic={
+            "basic": Traffic(
+                flows=[],
+                trips=[
+                    Trip(
+                        "other_interest",
+                        route=Route(
+                            begin=("edge-north-NS", 0, 20),
+                            end=("edge-south-NS", 0, "max"),
+                        ),
+                        depart=1,
+                    ),
+                ],
+            )
+        },
     ),
     output_dir=Path(__file__).parent,
 )

smarts/core/sensor.py

@@ -97,11 +97,20 @@ def __init__(
         self._follow_actor(
             vehicle_state, renderer
         )  # ensure we have a correct initial camera position
+        self._mask = mask
+        self._width = width
+        self._height = height
+        self._resolution = resolution
 
     def __eq__(self, __value: object) -> bool:
         return (
-            isinstance(__value, CameraSensor)
-            and self._target_actor == self._target_actor
+            isinstance(__value, self.__class__)
+            and self._target_actor == __value._target_actor
+            and self._camera_name == __value._camera_name
+            and self._mask == __value._mask
+            and self._width == __value._width
+            and self._height == __value._height
+            and self._resolution == __value._resolution
         )
 
     def teardown(self, **kwargs):

smarts/core/sensor_manager.py

@@ -21,12 +21,18 @@
 # THE SOFTWARE.
 import logging
 from collections import Counter
-from typing import Dict, FrozenSet, List, Optional, Set, Tuple
+from typing import Dict, Iterable, List, Optional, Set, Tuple
 
 from smarts.core import config
 from smarts.core.agent_interface import AgentInterface
 from smarts.core.renderer_base import RendererBase
-from smarts.core.sensors import Observation, Sensor, Sensors, SensorState
+from smarts.core.sensors import (
+    Observation,
+    Sensor,
+    SensorResolver,
+    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
@@ -49,7 +55,7 @@ def __init__(self):
         self._actors_by_sensor_id: Dict[str, Set[str]] = {}
         self._sensor_references = Counter()
         # {sensor_id, ...}
-        self._discarded_sensors: Set[str] = set()
+        self._scheduled_sensors: List[Sensor] = []
         observation_workers = config()(
             "core", "observation_workers", default=0, cast=int
         )
@@ -71,7 +77,7 @@ def __init__(self):
             raise LookupError(
                 f"SMARTS_CORE_SENSOR_PARALLELIZATION={backing} is not a valid option."
             )
-        self._sensor_resolver = (
+        self._sensor_resolver: SensorResolver = (
             parallel_resolver() if observation_workers > 0 else LocalSensorResolver()
         )
 
@@ -168,46 +174,61 @@ def teardown(self, renderer):
         self._sensor_states = {}
         self._sensors_by_actor_id = {}
         self._sensor_references.clear()
-        self._discarded_sensors.clear()
+        self._scheduled_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
+    def remove_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
             )
-            return frozenset()
+            return []
         del self._sensor_states[actor_id]
-        sensors_by_actor = self._sensors_by_actor_id[actor_id]
+        sensors_by_actor = self._sensors_by_actor_id.get(actor_id)
+        if not sensors_by_actor:
+            return []
+        discarded_sensors = []
         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)
+            self._sensor_references.subtract([sensor_id])
+            references = self._sensor_references[sensor_id]
+            discarded_sensors.append((self._sensors[sensor_id], references))
+            if references < 1:
+                self._disassociate_sensor(sensor_id, schedule_teardown)
         del self._sensors_by_actor_id[actor_id]
-        return frozenset(self._discarded_sensors)
+        return discarded_sensors
 
-    def remove_sensor(self, sensor_id: str) -> Optional[Sensor]:
+    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."""
         sensor = self._sensors.get(sensor_id)
         if not sensor:
             return None
+        self._disassociate_sensor(sensor_id, schedule_teardown)
+        return sensor
+
+    def _disassociate_sensor(self, sensor_id, schedule_teardown):
+        if schedule_teardown:
+            self._scheduled_sensors.append(self._sensors[sensor_id])
+
         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)
+                if sensors_ids := self._sensors_by_actor_id[actor_id]:
+                    sensors_ids.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."""
@@ -279,10 +300,7 @@ def clean_up_sensors_for_actors(self, current_actor_ids: Set[str], renderer):
         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)
+        for sensor in self._scheduled_sensors:
+            sensor.teardown(renderer=renderer)
 
-        self._discarded_sensors.clear()
+        self._scheduled_sensors.clear()

smarts/core/smarts.py

@@ -656,7 +656,12 @@ def switch_control_to_agent(
         plan = Plan(self.road_map, mission)
         interface = self.agent_manager.agent_interface_for_agent_id(agent_id)
         self.vehicle_index.start_agent_observation(
-            self, vehicle_id, agent_id, interface, plan
+            self,
+            vehicle_id,
+            agent_id,
+            interface,
+            plan,
+            initialize_sensors=not recreate,
         )
         vehicle = self.vehicle_index.switch_control_to_agent(
             self,

smarts/core/vehicle_index.py

@@ -371,18 +371,21 @@ def teardown(self, renderer):
 
     @clear_cache
     def start_agent_observation(
-        self, sim, vehicle_id, agent_id, agent_interface, plan, boid=False
+        self,
+        sim,
+        vehicle_id,
+        agent_id,
+        agent_interface,
+        plan,
+        boid=False,
+        initialize_sensors=True,
     ):
         """Associate an agent to a vehicle. Set up any needed sensor requirements."""
         original_agent_id = agent_id
         vehicle_id, agent_id = _2id(vehicle_id), _2id(agent_id)
+        self._2id_to_id[agent_id] = original_agent_id
 
         vehicle = self._vehicles[vehicle_id]
-        Vehicle.attach_sensors_to_vehicle(
-            sim.sensor_manager, sim, vehicle, agent_interface
-        )
-
-        self._2id_to_id[agent_id] = original_agent_id
 
         sim.sensor_manager.add_sensor_state(
             vehicle.id,
@@ -391,6 +394,10 @@ def start_agent_observation(
                 plan_frame=plan.frame(),
             ),
         )
+        if initialize_sensors:
+            Vehicle.attach_sensors_to_vehicle(
+                sim.sensor_manager, sim, vehicle, agent_interface
+            )
 
         self._controller_states[vehicle_id] = ControllerState.from_action_space(
             agent_interface.action, vehicle.pose, sim
@@ -402,9 +409,9 @@ def start_agent_observation(
             entity._replace(shadower_id=agent_id, is_boid=boid)
         )
 
-        # XXX: We are not giving the vehicle an AckermannChassis here but rather later
+        # XXX: We are not giving the vehicle a chassis here but rather later
         #      when we switch_to_agent_control. This means when control that requires
-        #      an AckermannChassis comes online, it needs to appropriately initialize
+        #      a chassis comes online, it needs to appropriately initialize
         #      chassis-specific states, such as tire rotation.
 
         return vehicle
@@ -622,6 +629,7 @@ def _switch_control_to_agent_recreate(
 
         # Remove the old vehicle
         self.teardown_vehicles_by_vehicle_ids([vehicle.id], sim.renderer_ref)
+        sim.sensor_manager.remove_sensors_by_actor_id(vehicle.id)
         # 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):