fix a few agents for RL e2e

ROAR/agent_module/agent.py

@@ -18,6 +18,7 @@
 import threading
 from ROAR.utilities_module.camera_models import Camera
 
+
 class Agent(ABC):
     """
     Abstract Agent class that define the minimum of a ROAR agent.

ROAR/agent_module/rl_depth_e2e_agent.py

@@ -1,12 +1,45 @@
-
+from ROAR.configurations.configuration import Configuration as AgentConfig
+from ROAR.control_module.pid_controller import PIDController
+from ROAR.planning_module.local_planner.loop_simple_waypoint_following_local_planner import LoopSimpleWaypointFollowingLocalPlanner
+from ROAR.planning_module.behavior_planner.behavior_planner import BehaviorPlanner
+from ROAR.planning_module.mission_planner.waypoint_following_mission_planner import WaypointFollowingMissionPlanner
+from pathlib import Path
+from ROAR.utilities_module.occupancy_map import OccupancyGridMap
+from ROAR.perception_module.obstacle_from_depth import ObstacleFromDepth
 from ROAR.agent_module.agent import Agent
 from ROAR.utilities_module.data_structures_models import SensorsData
 from ROAR.utilities_module.vehicle_models import Vehicle, VehicleControl
 
 
 class RLDepthE2EAgent(Agent):
+    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.pid_controller = PIDController(agent=self, steering_boundary=(-1, 1), throttle_boundary=(0, 1))
+        self.mission_planner = WaypointFollowingMissionPlanner(agent=self)
+        # initiated right after mission plan
+        self.behavior_planner = BehaviorPlanner(agent=self)
+        self.local_planner = LoopSimpleWaypointFollowingLocalPlanner(
+            agent=self,
+            controller=self.pid_controller,
+            mission_planner=self.mission_planner,
+            behavior_planner=self.behavior_planner,
+            closeness_threshold=1.5
+        )
+
+        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)
+
     def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
+        super(RLDepthE2EAgent, self).run_step(sensors_data, vehicle)
+        self.local_planner.run_in_series()
+        option = "obstacle_coords"  # ground_coords, obstacle_coords
+        if self.kwargs.get(option, None) is not None:
+            points = self.kwargs[option]
+            self.occupancy_map.update_async(points)
         if self.kwargs.get("control") is None:
             return VehicleControl()
         else:
-            return self.kwargs.get("control")
+            return self.kwargs.get("control")

ROAR/perception_module/obstacle_from_depth.py

@@ -53,7 +53,6 @@ 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
-            print(np.mean(points, axis=0), self.agent.vehicle.transform.location.y)
             obstacle_below_height_mask = \
                 np.abs(points[:, 1]) < self.agent.vehicle.transform.location.y + self.min_obstacle_height
             mask = obstacles_mask & obstacle_below_height_mask

ROAR/planning_module/local_planner/simple_waypoint_following_local_planner.py

@@ -90,9 +90,8 @@ def run_in_series(self) -> VehicleControl:
             return VehicleControl()
 
         # get vehicle's location
-        vehicle_transform: Union[Transform, None] = self.agent.vehicle.control
-
-        if vehicle_transform is None:
+        vehicle_transform: Union[Transform, None] = self.agent.vehicle.transform
+        if vehicle_transform is None or type(vehicle_transform) != Transform:
             raise AgentException("I do not know where I am, I cannot proceed forward")
 
         # redefine closeness level based on speed

ROAR/utilities_module/occupancy_map.py

@@ -27,7 +27,6 @@
 from PIL import Image
 
 
-
 class OccupancyGridMap(Module):
     def __init__(self, agent: Agent, **kwargs):
         """
@@ -129,7 +128,7 @@ def _update_grid_map_from_world_cord(self, world_cords_xy):
                                                                                     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[min_y:max_y, min_x:max_x] = 0  # free
                 self._map[occu_cords_y, occu_cords_x] += self._occu_prob
                 # self._map = self._map.clip(0, 1)
         except Exception as e: