1.local_planner_behavior.py介绍:
该模块主要逻辑就是基于pid控制建立一个简单的规划
2.代码位置以及引用
代码位于carla.agent.navigation
主要在我发布过的一篇文章里面被引用到:Carla模块介绍之BehaviorAgent类_sea_lichenglin的博客-CSDN博客
3.重要代码片段解读
(1)RoadOption(enum)
主要记录自车从一段路到另外一条车道的配置。
class RoadOption(Enum):
"""
RoadOption represents the possible topological configurations
when moving from a segment of lane to other.
"""
(2)LocalPlanner(object):
LocalPlanner实现跟踪轨迹的基本行为,动态生成的航路点。车辆的低水平运动通过使用两个PID控制器进行计算,一个用于横向控制另一个用于纵向控制(巡航速度)。当有多条路径可用时(交叉点)会进行一个随机选择。
class LocalPlanner(object):
"""
LocalPlanner implements the basic behavior of following a trajectory
of waypoints that is generated on-the-fly.
The low-level motion of the vehicle is computed by using two PID controllers,
one is used for the lateral control
and the other for the longitudinal control (cruise speed).
When multiple paths are available (intersections)
this local planner makes a random choice.
"""
(3)def __init__(self, agent):
为LocalPlanner里面的初始化函数,定义了一些参数
def __init__(self, agent):
"""
:param agent: agent that regulates the vehicle
:param vehicle: actor to apply to local planner logic onto
"""
self._vehicle = agent.vehicle
self._map = agent.vehicle.get_world().get_map()
self._target_speed = None
self.sampling_radius = None
self._min_distance = None
self._current_waypoint = None
self.target_road_option = None
self._next_waypoints = None
self.target_waypoint = None
self._vehicle_controller = None
self._global_plan = None
self._pid_controller = None
self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self._init_controller() # initializing controller
(4)def reset_vehicle(self):
重置车速函数
(5)def _init_controller(self):
初始化控制函数,该部分需要对pdi控制了解一些,具体了解的画,可以去百度上查查。该函数定义了两种pdi控制的参数,横向和纵向各两个。获取当前的路段,速度限制,最小安全距离。
def _init_controller(self):
"""
Controller initialization.
dt -- time difference between physics control in seconds.
This is can be fixed from server side
using the arguments -benchmark -fps=F, since dt = 1/F
target_speed -- desired cruise speed in km/h
min_distance -- minimum distance to remove waypoint from queue
lateral_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
# Default parameters
self.args_lat_hw_dict = {
'K_P': 0.75,
'K_D': 0.02,
'K_I': 0.4,
'dt': 1.0 / self.FPS}
self.args_lat_city_dict = {
'K_P': 0.58,
'K_D': 0.02,
'K_I': 0.5,
'dt': 1.0 / self.FPS}
self.args_long_hw_dict = {
'K_P': 0.37,
'K_D': 0.024,
'K_I': 0.032,
'dt': 1.0 / self.FPS}
self.args_long_city_dict = {
'K_P': 0.15,
'K_D': 0.05,
'K_I': 0.07,
'dt': 1.0 / self.FPS}
self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
self._global_plan = False
self._target_speed = self._vehicle.get_speed_limit()
self._min_distance = 3
(6)def set_speed(self, speed):
将获取到的速度,设置到控制物体里面。个人感觉有点多余,可能后续还可以开发吧。
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in km/h
"""
self._target_speed = speed
(7)def set_global_plan(self, current_plan, clean=False):
加载新的全局路径
def set_global_plan(self, current_plan, clean=False):
"""
Sets new global plan.
:param current_plan: list of waypoints in the actual plan
"""
for elem in current_plan:
self.waypoints_queue.append(elem)
if clean:
self._waypoint_buffer.clear()
for _ in range(self._buffer_size):
if self.waypoints_queue:
self._waypoint_buffer.append(
self.waypoints_queue.popleft())
else:
break
self._global_plan = True
(8)def get_incoming_waypoint_and_direction(self, steps=3):
获取进入道路的waypoint点和航向角,不清楚道路定义的,建议知乎看一下叶小飞老师的carla教程。
def get_incoming_waypoint_and_direction(self, steps=3):
"""
Returns direction and waypoint at a distance ahead defined by the user.
:param steps: number of steps to get the incoming waypoint.
"""
if len(self.waypoints_queue) > steps:
return self.waypoints_queue[steps]
else:
try:
wpt, direction = self.waypoints_queue[-1]
return wpt, direction
except IndexError as i:
print(i)
return None, RoadOption.VOID
return None, RoadOption.VOID
(9)def run_step(self, target_speed=None, debug=False):
重点,前面都是再做铺垫。在该函数进行速度的设置,依据条件选择一些pdi控制参数,最后调用VehiclePIDController类进行pdi控制,主要还是VehiclePIDController中的run_step函数。
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
if len(self.waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self.waypoints_queue:
self._waypoint_buffer.append(
self.waypoints_queue.popleft())
else:
break
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[0]
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
self._pid_controller = VehiclePIDController(self._vehicle,
args_lateral=args_lat,
args_longitudinal=args_long)
control = self._pid_controller.run_step(self._target_speed, self.target_waypoint)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if distance_vehicle(
waypoint, vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if debug:
draw_waypoints(self._vehicle.get_world(),
[self.target_waypoint], 1.0)
return control