working on rl agent

ROAR/agent_module/potential_field_agent.py

@@ -25,9 +25,12 @@ def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
         super().__init__(vehicle, agent_settings, **kwargs)
         self.route_file_path = Path(self.agent_settings.waypoint_file_path)
 
-        self.occupancy_map = OccupancyGridMap(agent=self)
-        occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy")
-        self.occupancy_map.load_from_file(occu_map_file_path)
+        self.occupancy_map = OccupancyGridMap(agent=self, threaded=True)
+        self.depth_to_obstacle = ObstacleFromDepth(agent=self, threaded=True)
+        self.add_threaded_module(self.occupancy_map)
+        self.add_threaded_module(self.depth_to_obstacle)
+        # occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy")
+        # self.occupancy_map.load_from_file(occu_map_file_path)
 
         self.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(0, 1))
         self.mission_planner = WaypointFollowingMissionPlanner(agent=self)
@@ -40,5 +43,7 @@ def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
 
     def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
         super().run_step(sensors_data=sensors_data, vehicle=vehicle)
+        if self.kwargs.get("obstacle_coords") is not None:
+            self.occupancy_map.update_async(self.kwargs.get("obstacle_coords"))
         control = self.local_planner.run_in_series()
         return control

ROAR/agent_module/rl_local_planner_agent.py

@@ -37,16 +37,8 @@ def __init__(self, target_speed=40, **kwargs):
             closeness_threshold=1.5
         )
         self.absolute_maximum_map_size, self.map_padding = 1000, 40
-        self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=self.absolute_maximum_map_size,
-                                              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.logger.debug(
@@ -59,13 +51,19 @@ def run_step(self, vehicle: Vehicle,
                                                   sensors_data=sensors_data)
         self.traditional_local_planner.run_in_series()
         self.transform_history.append(self.vehicle.transform)
-        if self.is_done:  # will never enter here
-            control = VehicleControl()
-            self.logger.debug("Path Following Agent is Done. Idling.")
-        else:
-            option = "obstacle_coords"  # ground_coords, point_cloud_obstacle_from_depth
-            if self.kwargs.get(option, None) is not None:
-                points = self.kwargs[option]
-                self.occupancy_map.update(points)
-            control = self.local_planner.run_in_series()
+        option = "obstacle_coords"  # ground_coords, point_cloud_obstacle_from_depth
+        if self.kwargs.get(option, None) is not None:
+            points = self.kwargs[option]
+            self.occupancy_map.update(points)
+        control = self.local_planner.run_in_series()
         return control
+
+    def get_obs(self):
+        ch1 = self.occupancy_map.get_map(transform=self.vehicle.transform,
+                                         view_size=(100, 100))
+        ch1 = np.expand_dims((ch1 * 255).astype(np.uint8), -1)
+        ch2 = np.zeros(shape=(100, 100, 1))
+        ch3 = np.zeros(shape=ch2.shape)
+        obs = np.concatenate([ch1, ch2, ch3], axis=2)
+        print(np.shape(obs))
+        return obs

ROAR/planning_module/local_planner/loop_simple_waypoint_following_local_planner.py

@@ -42,7 +42,7 @@ def __init__(
                          mission_planner=mission_planner,
                          behavior_planner=behavior_planner,
                          )
-        self.logger = logging.getLogger("SimplePathFollowingLocalPlanner")
+        self.logger = logging.getLogger("LoopSimplePathFollowingLocalPlanner")
 
         self.closeness_threshold = closeness_threshold
         self.closeness_threshold_config = json.load(Path(

ROAR/planning_module/local_planner/potential_field_planner.py

@@ -10,19 +10,21 @@
 from typing import Tuple
 from ROAR.planning_module.local_planner.loop_simple_waypoint_following_local_planner import \
     LoopSimpleWaypointFollowingLocalPlanner
+import logging
 
 
 class PotentialFieldPlanner(LoopSimpleWaypointFollowingLocalPlanner):
     def __init__(self, agent, **kwargs):
         super().__init__(agent, **kwargs)
+        self.logger = logging.getLogger("PotentialFieldPlanner")
         self.occu_map: OccupancyGridMap = self.agent.occupancy_map
         self._view_size = 50
         self.KP = 5.0  # attractive potential gain
         self.ETA = 2  # repulsive potential gain
         # the number of previous positions used to check oscillations
         self.OSCILLATIONS_DETECTION_LENGTH = 3
         self.AREA_WIDTH = 0
-        self.step_actions = np.array([[0,-1], [-1,-1], [1,-1]])
+        self.step_actions = np.array([[0, -1], [-1, -1], [1, -1]])
 
     def is_done(self):
         return False
@@ -40,8 +42,8 @@ def run_in_series(self) -> VehicleControl:
             obstacle_coords = np.where(m > 0.5)
             me_coord = np.where(m == -10)
             sx, sy = me_coord[0][0], me_coord[1][0]
-            gx, gy = np.clip(self._view_size // 2 + gx, 0, self._view_size-1), \
-                     np.clip(0 + gy, 0, self._view_size-1)
+            gx, gy = np.clip(self._view_size // 2 + gx, 0, self._view_size - 1), \
+                     np.clip(0 + gy, 0, self._view_size - 1)
             ox, oy = obstacle_coords
 
             rx, ry = self.potential_field_planning(sx=sx, sy=sy, gx=gx, gy=gy, ox=ox, oy=oy,
@@ -116,10 +118,9 @@ def calc_repulsive_potential_vec(self, world: np.ndarray, ox: np.ndarray, oy: np
             o_s = 2
             for x, y in zip(ox, oy):
                 # increase potential value of points around obstacles
-                world[x-o_s:x+o_s, y-o_s:y+o_s] += 0.5 * self.ETA * (1 / 1.13 - 1 / rr) ** 2
+                world[x - o_s:x + o_s, y - o_s:y + o_s] += 0.5 * self.ETA * (1 / 1.13 - 1 / rr) ** 2
                 world[x][y] += 0.5 * self.ETA * (1 / 0.1 - 1 / rr) ** 2
 
-
             # obstacle_coords = np.array(list(zip(ox, oy)))
             # indices = indices.reshape((2, indices.shape[1] * indices.shape[2])).T
             # for x, y in indices:

ROAR/planning_module/local_planner/rl_local_planner.py

@@ -44,11 +44,6 @@ def is_done(self) -> bool:
         return False
 
     def run_in_series(self) -> VehicleControl:
-        next_waypoint: Transform = self.agent.kwargs.get("next_waypoint", None)
-        if next_waypoint is None:
-            return VehicleControl()
-        else:
-            self.way_points_queue.append(next_waypoint)
-            control = self.controller.run_in_series(next_waypoint=next_waypoint)
-            return control
+        control: VehicleControl = self.agent.kwargs.get("control", VehicleControl())
+        return control
 

runner_sim.py

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