fix cache indexing and ground_truth_cache_cloned

Author: Milotrince

genesis/sensors/sensor_manager.py

@@ -23,7 +23,7 @@ def __init__(self, sim):
         self._cache: dict[Type[torch.dtype], TensorRingBuffer] = {}
         self._cache_slices_by_type: dict[Type["Sensor"], slice] = {}
 
-        self._last_ground_truth_cache_cloned_step: int = -1
+        self._last_ground_truth_cache_cloned_step: dict[Type[torch.dtype], int] = {}
         self._cloned_ground_truth_cache: dict[Type[torch.dtype], torch.Tensor] = {}
 
     def create_sensor(self, sensor_options: "SensorOptions"):
@@ -78,7 +78,7 @@ def build(self):
             dtype = sensor_cls._get_cache_dtype()
             for sensor in sensors:
                 sensor._shared_metadata = self._sensors_metadata[sensor_cls]
-                sensor._cache = self._cache[dtype][sensor._cache_idx : sensor._cache_idx + sensor._cache_size]
+                sensor._cache = self._cache[dtype][:, sensor._cache_idx : sensor._cache_idx + sensor._cache_size]
                 sensor.build()
 
     def step(self):
@@ -96,8 +96,8 @@ def step(self):
 
     def get_cloned_from_ground_truth_cache(self, sensor: "Sensor") -> torch.Tensor:
         dtype = sensor._get_cache_dtype()
-        if self._last_ground_truth_cache_cloned_step != self._sim.cur_step_global:
-            self._last_ground_truth_cache_cloned_step = self._sim.cur_step_global
+        if self._last_ground_truth_cache_cloned_step[dtype] != self._sim.cur_step_global:
+            self._last_ground_truth_cache_cloned_step[dtype] = self._sim.cur_step_global
             self._cloned_ground_truth_cache[dtype] = self._ground_truth_cache[dtype].clone()
         return self._cloned_ground_truth_cache[dtype][:, sensor._cache_idx : sensor._cache_idx + sensor._cache_size]