diff --git a/ROAR/agent_module/potential_field_agent.py b/ROAR/agent_module/potential_field_agent.py index e8ce8040..5ca27c2a 100644 --- a/ROAR/agent_module/potential_field_agent.py +++ b/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 diff --git a/ROAR/agent_module/rl_local_planner_agent.py b/ROAR/agent_module/rl_local_planner_agent.py index 3cff2ba7..1a553b0a 100644 --- a/ROAR/agent_module/rl_local_planner_agent.py +++ b/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 diff --git a/ROAR/planning_module/local_planner/loop_simple_waypoint_following_local_planner.py b/ROAR/planning_module/local_planner/loop_simple_waypoint_following_local_planner.py index 2da83ff7..1bd23490 100644 --- a/ROAR/planning_module/local_planner/loop_simple_waypoint_following_local_planner.py +++ b/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( diff --git a/ROAR/planning_module/local_planner/potential_field_planner.py b/ROAR/planning_module/local_planner/potential_field_planner.py index 3bde680f..3f2eeae3 100644 --- a/ROAR/planning_module/local_planner/potential_field_planner.py +++ b/ROAR/planning_module/local_planner/potential_field_planner.py @@ -10,11 +10,13 @@ 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 @@ -22,7 +24,7 @@ def __init__(self, agent, **kwargs): # 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: diff --git a/ROAR/planning_module/local_planner/rl_local_planner.py b/ROAR/planning_module/local_planner/rl_local_planner.py index 6f0dccd3..59ca94e3 100644 --- a/ROAR/planning_module/local_planner/rl_local_planner.py +++ b/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 diff --git a/ROAR_Gym b/ROAR_Gym index e45400f4..95a26a51 160000 --- a/ROAR_Gym +++ b/ROAR_Gym @@ -1 +1 @@ -Subproject commit e45400f49c817d821bc3e992a1213f8860a31718 +Subproject commit 95a26a517a2a28a88e1b2ceec27a247fbaead1ef diff --git a/ROAR_Jetson b/ROAR_Jetson index b8db94a4..313a16d2 160000 --- a/ROAR_Jetson +++ b/ROAR_Jetson @@ -1 +1 @@ -Subproject commit b8db94a4d8575eba217a9c6eba8fa4bbc00f7b58 +Subproject commit 313a16d2ec83f669166efbbe3d750398e38ab1ae diff --git a/ROAR_Sim b/ROAR_Sim index fe648608..9a405db5 160000 --- a/ROAR_Sim +++ b/ROAR_Sim @@ -1 +1 @@ -Subproject commit fe648608738b4175d6a41a26e27d60012cf64959 +Subproject commit 9a405db5b769c3df8f316dcded52452b0ad81114 diff --git a/runner_sim.py b/runner_sim.py index 16e2e3e0..354b846e 100644 --- a/runner_sim.py +++ b/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()