cant get the log odd update working correctly

ROAR/agent_module/occupancy_map_agent.py

@@ -36,16 +36,9 @@ def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
             behavior_planner=self.behavior_planner,
             closeness_threshold=1.5
         )
-        self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=800,
-                                              world_coord_resolution=1,
-                                              occu_prob=0.99,
-                                              max_points_to_convert=10000,
-                                              threaded=True)
-        self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self,
-                                                              threaded=True,
-                                                              max_detectable_distance=0.5,
-                                                              max_points_to_convert=20000,
-                                                              min_obstacle_height=2)
+
+        self.occupancy_map = OccupancyGridMap(agent=self, threaded=True)
+        self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self, threaded=True)
         self.add_threaded_module(self.obstacle_from_depth_detector)
         self.add_threaded_module(self.occupancy_map)
         # self.vis = o3d.visualization.Visualizer()
@@ -60,32 +53,8 @@ def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleContro
         if self.kwargs.get(option, None) is not None:
             points = self.kwargs[option]
             self.occupancy_map.update_async(points)
-            arb_points = [self.local_planner.way_points_queue[0].location]
-            m = self.occupancy_map.get_map(transform=self.vehicle.transform,
-                                           view_size=(200, 200),
-                                           vehicle_value=-1,
-                                           arbitrary_locations=arb_points,
-                                           arbitrary_point_value=-5)
-            # print(np.where(m == -5))
-            # cv2.imshow("m", m)
-            # cv2.waitKey(1)
-            # occu_map_vehicle_center = np.array(list(zip(*np.where(m == np.min(m))))[0])
-            # correct_next_waypoint_world = self.local_planner.way_points_queue[0]
-            # diff = np.array([correct_next_waypoint_world.location.x,
-            #                  correct_next_waypoint_world.location.z]) - \
-            #        np.array([self.vehicle.transform.location.x,
-            #                  self.vehicle.transform.location.z])
-            # correct_next_waypoint_occu = occu_map_vehicle_center + diff
-            # correct_next_waypoint_occu = np.array([49.97, 44.72596359])
-            # estimated_world_coord = self.occupancy_map.cropped_occu_to_world(
-            #     cropped_occu_coord=correct_next_waypoint_occu, vehicle_transform=self.vehicle.transform,
-            #     occu_vehicle_center=occu_map_vehicle_center)
-            # print(f"correct o-> {correct_next_waypoint_occu}"
-            #       f"correct w-> {correct_next_waypoint_world.location} | "
-            #       f"estimated = {estimated_world_coord.location.x}")
-            # cv2.imshow("m", m)
-            # cv2.waitKey(1)
-            # print()
+            self.occupancy_map.visualize()
+            # self.occupancy_map.visualize(transform=self.vehicle.transform, view_size=(200, 200), vehicle_value=1)
 
             # if self.points_added is False:
             #     self.pcd = o3d.geometry.PointCloud()

ROAR/configurations/configuration.py

@@ -76,6 +76,9 @@ class Configuration(BaseModel):
     target_speed: int = 80
     pid_config_file_path: str = Field(default="./ROAR_Sim/configurations/pid_config.json")
     lqr_config_file_path: str = Field(default="./ROAR_Sim/configurations/lqr_config.json")
+    occu_map_config_path: str = Field(default="./ROAR_Sim/configurations/occu_map_config.json")
+    obstacle_from_depth_config_path: str = Field(default="./ROAR_Sim/configurations/obstacle_from_depth_config.json")
+
 
     simple_waypoint_local_planner_config_file_path: str = \
         Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json")

ROAR/perception_module/obstacle_from_depth.py

@@ -5,23 +5,20 @@
 import numpy as np
 import cv2
 import open3d
+from pydantic import BaseModel, Field
 
 
 class ObstacleFromDepth(Detector):
     def save(self, **kwargs):
         pass
 
-    def __init__(self, agent: Agent,
-                 max_detectable_distance: float = 0.3,
-                 max_points_to_convert: int = 10000,
-                 max_incline_normal=0.5,
-                 min_obstacle_height: float = 3, ** kwargs):
-
+    def __init__(self, agent: Agent, **kwargs):
         super().__init__(agent, **kwargs)
-        self.max_detectable_distance = kwargs.get("max_detectable_distance", max_detectable_distance)
-        self.max_points_to_convert = kwargs.get("max_points_to_convert", max_points_to_convert)
-        self.max_incline_normal = kwargs.get("max_incline_normal", max_incline_normal)
-        self.min_obstacle_height = kwargs.get("max_obstacle_height", min_obstacle_height)
+        config = ObstacleFromDepthConfig.parse_file(self.agent.agent_settings.obstacle_from_depth_config_path)
+        self.max_detectable_distance = kwargs.get("max_detectable_distance", config.max_detectable_distance)
+        self.max_points_to_convert = kwargs.get("max_points_to_convert", config.max_points_to_convert)
+        self.max_incline_normal = kwargs.get("max_incline_normal", config.max_incline_normal)
+        self.min_obstacle_height = kwargs.get("max_obstacle_height", config.min_obstacle_height)
 
     def run_in_series(self, **kwargs) -> Any:
         if self.agent.front_depth_camera.data is not None:
@@ -56,7 +53,8 @@ def run_in_series(self, **kwargs) -> Any:
             normals = np.asarray(pcd.normals)
             abs_normals = np.abs(normals)
             obstacles_mask = abs_normals[:, 1] < self.max_incline_normal
-            obstacle_below_height_mask = points[:, 1] < self.agent.vehicle.transform.location.y + self.min_obstacle_height
+            obstacle_below_height_mask = points[:,
+                                         1] < self.agent.vehicle.transform.location.y + self.min_obstacle_height
             mask = obstacles_mask & obstacle_below_height_mask
             self.agent.kwargs["point_cloud_obstacle_from_depth"] = points
             self.agent.kwargs["obstacle_coords"] = points[mask]
@@ -70,3 +68,11 @@ def _pix2xyz(depth_img, i, j):
             depth_img[i, j] * i * 1000,
             depth_img[i, j] * 1000
         ]
+
+
+class ObstacleFromDepthConfig(BaseModel):
+    max_detectable_distance: float = Field(default=0.3)
+    max_points_to_convert: int = Field(default=10000)
+    max_incline_normal: float = Field(default=0.5)
+    min_obstacle_height: float = Field(default=3)
+    update_interval: float = Field(default=0.1)

ROAR/utilities_module/module.py

@@ -36,10 +36,13 @@ def run_in_threaded(self, **kwargs):
 
         """
         while self.should_continue_threaded:
+            start = time.time()
             self.run_in_series()
             if self.should_save:
                 self.save()
-            time.sleep(self.update_interval)
+            end = time.time()
+            if end - start < self.update_interval * 1000:
+                time.sleep((end-start)*0.001)
 
     def shutdown(self):
         self.should_continue_threaded = False

ROAR/utilities_module/occupancy_map.py

@@ -21,33 +21,13 @@
 from collections import deque
 import itertools
 import time
+from ROAR.agent_module.agent import Agent
+from pydantic import BaseModel, Field
+import json
 
 
 class OccupancyGridMap(Module):
-    """
-    A 2D Occupancy Grid Map representing the world
-    Should be able to handle
-        1. Transformation of coordinate from world to grid cord
-        2. Transformation of cord from grid cord to world cord
-        3. Represent the vehicle size and position, vehicle is represented as 0
-            Note that this class does NOT remember the vehicle position,
-            in order to visualize vehicle, vehicle position has to be passed in
-        4. Represent the obstacles' position once its world coordinate is
-           given
-        5. Represent free space's position, once its world coordinate is given
-        6. visualize itself, including zoom to a certain area so that not
-           the entire map is visualized
-        7. The range of values should be bewteen 0 - 1
-            - 0 = obstacles, 1 = free space
-        8 Failure tolerant, if I pass in a wrong world coordinate,
-          it will prompt it, but do not fail. Similar with other functions
-          in this class
-        9. Fixed map size for enhanced performance
-    """
-
-    def __init__(self, absolute_maximum_map_size=10000, map_padding: int = 40, vehicle_width=2, vehicle_height=2,
-                 world_coord_resolution=1, occu_prob: float = 0.95, free_prob: float = 0.05,
-                 max_points_to_convert: int = 1000, **kwargs):
+    def __init__(self, agent: Agent, **kwargs):
         """
         Args:
             absolute_maximum_map_size: Absolute maximum size of the map, will be used to compute a square occupancy map
@@ -60,26 +40,28 @@ def __init__(self, absolute_maximum_map_size=10000, map_padding: int = 40, vehic
         """
         super().__init__(name="occupancy_map", **kwargs)
         self.logger = logging.getLogger(__name__)
+        self._agent = agent
+        config = OccupancyGridMapConfig.parse_file(self._agent.agent_settings.occu_map_config_path)
         self._map: Optional[np.ndarray] = None
-        self._world_coord_resolution = world_coord_resolution
-        self._absolute_maximum_map_size = absolute_maximum_map_size
+        self._world_coord_resolution = config.world_coord_resolution
+        self._absolute_maximum_map_size = config.absolute_maximum_map_size
 
         self._min_x = -math.floor(self._absolute_maximum_map_size)
         self._min_y = -math.floor(self._absolute_maximum_map_size)
 
         self._max_x = math.ceil(self._absolute_maximum_map_size)
         self._max_y = math.ceil(self._absolute_maximum_map_size)
 
-        self._map_additiona_padding = map_padding
+        self._map_additiona_padding = config.map_padding
 
-        self._vehicle_width = vehicle_width
-        self._vehicle_height = vehicle_height
+        self._vehicle_width = config.vehicle_width
+        self._vehicle_height = config.vehicle_height
 
         self._initialize_map()
-        self._occu_prob = np.log(occu_prob / (1 - occu_prob))
-        self._free_prob = 1 - self._occu_prob
+        self._occu_prob = np.clip(np.log(config.occu_prob / (1 - config.occu_prob)), 0, 1)
+        self._free_prob = - (1 - self._occu_prob)
 
-        self._max_points_to_convert = max_points_to_convert
+        self._max_points_to_convert = config.max_points_to_convert
         self.curr_obstacle_world_coords = None
         self._curr_obstacle_occu_coords = None
         self._static_obstacles: Optional[np.ndarray] = None
@@ -136,22 +118,33 @@ def _update_grid_map_from_world_cord(self, world_cords_xy):
         try:
             # self.logger.debug(f"Updating Grid Map: {np.shape(world_cords_xy)}")
             # print(f"Updating Grid Map: {np.shape(world_cords_xy)}")
-
-            self._curr_obstacle_occu_coords = self.cord_translation_from_world(
-                world_cords_xy=world_cords_xy)
-            occu_cords_x, occu_cords_y = self._curr_obstacle_occu_coords[:, 0], self._curr_obstacle_occu_coords[:, 1]
-            min_x, max_x, min_y, max_y = np.min(occu_cords_x), np.max(occu_cords_x), \
-                                         np.min(occu_cords_y), np.max(occu_cords_y)
-            self._map[min_y:max_y, min_x:max_x] = 0  # free
-            self._map[occu_cords_y, occu_cords_x] = 1  # occupied
+            # print(len(self._curr_obstacle_occu_coords))
+            if world_cords_xy is not None and len(world_cords_xy) > 0:
+                self._curr_obstacle_occu_coords = self.cord_translation_from_world(
+                    world_cords_xy=world_cords_xy)
+                occu_cords_x, occu_cords_y = self._curr_obstacle_occu_coords[:, 0], \
+                                             self._curr_obstacle_occu_coords[:, 1]
+                # simulation
+                # real life -> fake log odd update
+                view_width, view_height = 100, 100
+                occu_loc = self.location_to_occu_cord(self._agent.vehicle.transform.location)[0]
+                x, y = occu_loc
+                min_x, min_y, max_x, max_y = np.array([x - view_width // 2, y - view_height // 2,
+                                                       x + view_width // 2, y + view_height // 2])
+                # print(self._free_prob, self._occu_prob)
+                # self._map[min_y: max_y, min_x: max_x] += self._free_prob
+                self._map[occu_cords_y, occu_cords_x] += self._occu_prob
+                # self._map = self._map.clip(0, 1)
         except Exception as e:
             self.logger.error(f"Unable to update: {e}")
-        # activate the below three line in real world due to sensor error
-        # min_occu_cords_x, max_occu_cords_x = np.min(occu_cords_x), np.max(occu_cords_x)
-        # min_occu_cords_y, max_occu_cords_y = np.min(occu_cords_y), np.max(occu_cords_y)
-        # self.map[min_occu_cords_y: max_occu_cords_y, min_occu_cords_x:max_occu_cords_x] += self.free_prob
-        # self.map[occu_cords_y, occu_cords_x] += self.occu_prob
-        # self.map = self.map.clip(0, 1)
+
+    def _get_rot_matrix(self) -> np.ndarray:
+        yaw = np.deg2rad(self._agent.vehicle.transform.rotation.yaw)
+        R = np.array([
+            [np.cos(yaw), -np.sin(yaw)],
+            [np.sin(yaw), np.cos(yaw)]
+        ])
+        return R
 
     def update(self, world_coords: np.ndarray):
         """
@@ -168,10 +161,6 @@ def update(self, world_coords: np.ndarray):
         world_coords_xy = world_coords[:, [0, 2]] * self._world_coord_resolution
         self._update_grid_map_from_world_cord(world_cords_xy=world_coords_xy)
 
-    def record(self, map_xs, map_ys):
-        m: np.ndarray = np.zeros(shape=np.shape(self._map))
-        m[map_ys, map_xs] = 1
-
     def run_in_series(self, **kwargs):
         if self.curr_obstacle_world_coords is not None:
             self.update(world_coords=self.curr_obstacle_world_coords)
@@ -203,7 +192,8 @@ def save(self, **kwargs):
 
     def visualize(self,
                   transform: Optional[Transform] = None,
-                  view_size: Tuple[int, int] = (10, 10)):
+                  view_size: Tuple[int, int] = (10, 10),
+                  vehicle_value: Optional[int] = None):
         """
         if transform is None:
             Visualize the entire map, output size constraint to (500,500)
@@ -216,7 +206,7 @@ def visualize(self,
         Returns:
 
         """
-        curr_map = self.get_map(transform=transform, view_size=view_size)
+        curr_map = self.get_map(transform=transform, view_size=view_size, vehicle_value=vehicle_value)
         try:
             cv2.imshow("Occupancy Grid Map", cv2.resize(np.float32(curr_map), dsize=(500, 500)))
             cv2.waitKey(1)
@@ -305,3 +295,14 @@ def load_from_file(self, file_path: Path):
                                            f"does not match the expected shape [{self._map.shape}]"
         self._map = m
         self._static_obstacles = np.vstack([np.where(self._map == 1)]).T
+
+
+class OccupancyGridMapConfig(BaseModel):
+    absolute_maximum_map_size: int = Field(default=10000)
+    map_padding: int = Field(default=40)
+    vehicle_height: int = Field(default=2)
+    vehicle_width: int = Field(default=2)
+    world_coord_resolution: int = Field(default=1)
+    occu_prob: float = Field(default=0.7)
+    max_points_to_convert: int = Field(default=1000)
+    update_interval: float = Field(default=0.1)

runner_jetson.py

@@ -8,7 +8,7 @@
 # from ROAR.agent_module.special_agents.waypoint_generating_agent import WaypointGeneratigAgent
 # from ROAR.agent_module.forward_only_agent import ForwardOnlyAgent
 # from ROAR.agent_module.lane_detection_agent import LaneDetectionAgent
-# from ROAR.agent_module.occupancy_map_agent import OccupancyMapAgent
+from ROAR.agent_module.occupancy_map_agent import OccupancyMapAgent
 # from ROAR.agent_module.special_agents.recording_agent import RecordingAgent
 from ROAR.agent_module.michael_pid_agent import PIDAgent
 from pathlib import Path
@@ -31,7 +31,7 @@ def main(auto=False):
             prepare(jetson_config=jetson_config)
         except Exception as e:
             logging.error(f"Ignoring Error during setup: {e}")
-        agent = PIDAgent(vehicle=Vehicle(), agent_settings=agent_config, should_init_default_cam=False)
+        agent = OccupancyMapAgent(vehicle=Vehicle(), agent_settings=agent_config, should_init_default_cam=False)
         jetson_runner = JetsonRunner(agent=agent, jetson_config=jetson_config)
         jetson_runner.start_game_loop(use_manual_control=not auto)
     except Exception as e:

runner_sim.py

@@ -20,7 +20,7 @@ def main():
                                npc_agent_class=PurePursuitAgent)
     try:
         my_vehicle = carla_runner.set_carla_world()
-        agent = PIDAgent(vehicle=my_vehicle, agent_settings=agent_config)
+        agent = OccupancyMapAgent(vehicle=my_vehicle, agent_settings=agent_config)
         carla_runner.start_game_loop(agent=agent, use_manual_control=True)
     except Exception as e:
         logging.error(f"Something bad happened during initialization: {e}")