def run_in_series(self) -> VehicleControl:
"""
Run step for the local planner
Procedure:
1. Sync data
2. get the correct look ahead for current speed
3. get the correct next waypoint
4. feed waypoint into controller
5. return result from controller
Returns:
next control that the local think the agent should execute.
"""
if (len(self.mission_planner.mission_plan) == 0
and len(self.way_points_queue) == 0):
return VehicleControl()
# get vehicle's location
vehicle_transform: Union[Transform,
None] = self.agent.vehicle.transform
if vehicle_transform is None:
raise AgentException(
"I do not know where I am, I cannot proceed forward")
# redefine closeness level based on speed
self.set_closeness_threhold(self.closeness_threshold_config)
# get current waypoint
curr_closest_dist = float("inf")
while True:
if len(self.way_points_queue) == 0:
self.logger.info("Destination reached")
return VehicleControl()
# waypoint: Transform = self.way_points_queue[0]
waypoint, speed_factor = self.next_waypoint_smooth_and_speed()
curr_dist = vehicle_transform.location.distance(waypoint.location)
if curr_dist < curr_closest_dist:
# if i find a waypoint that is closer to me than before
# note that i will always enter here to start the calculation for curr_closest_dist
curr_closest_dist = curr_dist
elif curr_dist < self.closeness_threshold:
# i have moved onto a waypoint, remove that waypoint from the queue
self.way_points_queue.popleft()
else:
break
target_waypoint, speed_factor = self.next_waypoint_smooth_and_speed()
control: VehicleControl = self.controller.run_in_series(
next_waypoint=target_waypoint, speed_multiplier=speed_factor)
self.logger.debug(
f"\n"
f"Curr Transform: {self.agent.vehicle.transform}\n"
f"Target Location: {target_waypoint.location}\n"
f"Control: {control} | Speed: {Vehicle.get_speed(self.agent.vehicle)}\n"
)
return control
def run_in_series(self) -> VehicleControl:
# get vehicle's location
vehicle_transform: Union[Transform, None] = self.agent.vehicle.control
if vehicle_transform is None:
raise AgentException(
"I do not know where I am, I cannot proceed forward")
target_waypoint = self.find_next_waypoint()
control: VehicleControl = self.controller.run_in_series(
next_waypoint=target_waypoint)
# self.logger.debug(f"control -> {control} | next waypoint -> {target_waypoint.location}")
return control
def run_in_series(self) -> VehicleControl:
# get vehicle's location
vehicle_transform: Union[Transform,
None] = self.agent.vehicle.transform
if vehicle_transform is None:
raise AgentException(
"I do not know where I am, I cannot proceed forward")
# redefine closeness level based on speed
self.set_closeness_threhold(self.closeness_threshold_config)
# get current waypoint
curr_closest_dist = float("inf")
while True:
if len(self.way_points_queue) == self._curr_waypoint_index:
self._curr_waypoint_index = 0 + 10 # this is so that i don't actually just look at the zeroth one
# when i loop back
waypoint: Transform = self.way_points_queue[
self._curr_waypoint_index]
curr_dist = vehicle_transform.location.distance(waypoint.location)
if curr_dist < curr_closest_dist:
# if i find a waypoint that is closer to me than before
# note that i will always enter here to start the calculation for curr_closest_dist
curr_closest_dist = curr_dist
elif curr_dist < self.closeness_threshold:
# i have moved onto a waypoint, remove that waypoint from the queue
self._curr_waypoint_index += 1
else:
break
target_waypoint = self.way_points_queue[self._curr_waypoint_index]
# print(target_waypoint)
control: VehicleControl = self.controller.run_in_series(
next_waypoint=target_waypoint)
# control = 0
return control
def run_step(self) -> VehicleControl:
"""
Run step for the local planner
Procedure:
1. Sync data
2. get the correct look ahead for current speed
3. get the correct next waypoint
4. feed waypoint into controller
5. return result from controller
Returns:
next control that the local think the agent should execute.
"""
if (
len(self.mission_planner.mission_plan) == 0
and len(self.way_points_queue) == 0
):
return VehicleControl()
# get vehicle's location
vehicle_transform: Union[Transform, None] = self.agent.vehicle.transform
if vehicle_transform is None:
raise AgentException("I do not know where I am, I cannot proceed forward")
# redefine closeness level based on speed
curr_speed = Vehicle.get_speed(self.agent.vehicle)
if curr_speed < 60:
self.closeness_threshold = 5
elif curr_speed < 80:
self.closeness_threshold = 15
elif curr_speed < 120:
self.closeness_threshold = 20
else:
self.closeness_threshold = 50
# print(f"Curr closeness threshold = {self.closeness_threshold}")
# get current waypoint
curr_closest_dist = float("inf")
while True:
if len(self.way_points_queue) == 0:
self.logger.info("Destination reached")
return VehicleControl()
waypoint: Transform = self.way_points_queue[0]
curr_dist = vehicle_transform.location.distance(waypoint.location)
if curr_dist < curr_closest_dist:
# if i find a waypoint that is closer to me than before
# note that i will always enter here to start the calculation for curr_closest_dist
curr_closest_dist = curr_dist
elif curr_dist < self.closeness_threshold:
# i have moved onto a waypoint, remove that waypoint from the queue
self.way_points_queue.popleft()
else:
break
target_waypoint = self.way_points_queue[0]
# target_waypoint = Transform.average(self.way_points_queue[0], self.way_points_queue[1])
# target_waypoint = Transform.average(self.way_points_queue[2], target_waypoint)
control: VehicleControl = self.controller.run_step(next_waypoint=target_waypoint)
# self.logger.debug(
# f"Target_Location {target_waypoint.location} "
# f"| Curr_Location {vehicle_transform.location} "
# f"| Distance {int(curr_closest_dist)}")
return control
