class OccuMapDemoDrivingAgent(Agent):
def __init__(self, vehicle: Vehicle, agent_settings: AgentConfig, **kwargs):
super().__init__(vehicle, agent_settings, **kwargs)
self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=550,
world_coord_resolution=1,
occu_prob=0.99,
max_points_to_convert=5000)
occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy")
self.occupancy_map.load_from_file(file_path=occu_map_file_path)
def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl:
super(OccuMapDemoDrivingAgent, self).run_step(sensors_data=sensors_data, vehicle=vehicle)
self.occupancy_map.visualize(transform=self.vehicle.transform, view_size=(200, 200))
return VehicleControl()
class MarkAgent(Agent):
def __init__(self, **kwargs):
super().__init__(**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 = DynamicWindowsApproach(
agent=self,
controller=self.pid_controller)
occu_map_file_path = Path("./ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy")
self.occupancy_map = OccupancyGridMap(absolute_maximum_map_size=550,
world_coord_resolution=1,
occu_prob=0.99,
max_points_to_convert=5000,
threaded=True)
self.occupancy_map.load_from_file(file_path=occu_map_file_path)
# self.obstacle_from_depth_detector = ObstacleFromDepth(agent=self,
# threaded=True,
# max_detectable_distance=0.3,
# max_points_to_convert=10000,
# min_obstacle_height=2)
# 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(MarkAgent, self).run_step(vehicle=vehicle,
sensors_data=sensors_data)
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_async(points)
self.occupancy_map.visualize()
self.occupancy_map.get_map()
return control
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)
# the part about visualization
self.occupancy_map = OccupancyGridMap(agent=self, threaded=True)
occ_file_path = Path(
"../ROAR_Sim/data/easy_map_cleaned_global_occu_map.npy")
self.occupancy_map.load_from_file(occ_file_path)
self.plan_lst = list(
self.mission_planner.produce_single_lap_mission_plan())
self.kwargs = kwargs
self.interval = self.kwargs.get('interval', 50)
self.look_back = self.kwargs.get('look_back', 5)
self.look_back_max = self.kwargs.get('look_back_max', 10)
self.thres = self.kwargs.get('thres', 1e-3)
self.int_counter = 0
self.counter = 0
self.finished = False
self.curr_dist_to_strip = 0
self.bbox: Optional[LineBBox] = None
self._get_next_bbox()
def run_step(self, sensors_data: SensorsData,
vehicle: Vehicle) -> VehicleControl:
super(RLDepthE2EAgent, self).run_step(sensors_data, vehicle)
self.local_planner.run_in_series()
_, self.curr_dist_to_strip = self.bbox_step()
if self.kwargs.get("control") is None:
return VehicleControl()
else:
return self.kwargs.get("control")
def bbox_step(self):
"""
This is the function that the line detection agent used
Main function to use for detecting whether the vehicle reached a new strip in
the current step. The old strip (represented as a bbox) will be gone forever
return:
crossed: a boolean value indicating whether a new strip is reached
dist (optional): distance to the strip, value no specific meaning
"""
self.counter += 1
if not self.finished:
crossed, dist = self.bbox.has_crossed(self.vehicle.transform)
if crossed:
self.int_counter += 1
self._get_next_bbox()
return crossed, dist
return False, 0.0
def _get_next_bbox(self):
# make sure no index out of bound error
curr_lb = self.look_back
curr_idx = self.int_counter * self.interval
while curr_idx + curr_lb < len(self.plan_lst):
if curr_lb > self.look_back_max:
self.int_counter += 1
curr_lb = self.look_back
curr_idx = self.int_counter * self.interval
continue
t1 = self.plan_lst[curr_idx]
t2 = self.plan_lst[curr_idx + curr_lb]
dx = t2.location.x - t1.location.x
dz = t2.location.z - t1.location.z
if abs(dx) < self.thres and abs(dz) < self.thres:
curr_lb += 1
else:
self.bbox = LineBBox(t1, t2)
return
# no next bbox
print("finished all the iterations!")
self.finished = True