feat: 给refer_path添加注释
parent
ef6e7c7589
commit
03d49805a4
|
|
@ -257,11 +257,11 @@ class ReferencePath:
|
|||
angle = np.mod(wp.psi - math.pi / 2 + math.pi,
|
||||
2 * math.pi) - math.pi
|
||||
# Get closest cell to orthogonal vector
|
||||
# 获取与正交向量最近的网格单元
|
||||
# 获取与正交向量最近的网格单元，将方向向量，转换为两个离散的单元点，用于下面的最短距离查找
|
||||
t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y
|
||||
+ max_width * np.sin(angle))
|
||||
# Compute distance to orthogonal cell on path border
|
||||
# 计算到路径边界上正交网格单元的距离
|
||||
# 计算到路径边界上正交网格单元的距离，使用bresenham算法离散查找最近的障碍物点（也可能是边界点）
|
||||
b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width)
|
||||
# Add information to list for current waypoint
|
||||
# 将当前路径点的信息添加到列表中
|
||||
|
|
@ -271,7 +271,7 @@ class ReferencePath:
|
|||
# Set waypoint attributes with width to the left and right
|
||||
# 设置路径点的左右两侧宽度
|
||||
wp.ub = width_info[0]
|
||||
wp.lb = -1 * width_info[2] # minus can be assumed as waypoints 负号可以认为是路径点
|
||||
wp.lb = -1 * width_info[2] # minus can be assumed as waypoints 负号的含义是在右侧的宽度，相对于中心线而言，左侧为正，右侧为负
|
||||
# represent center-line of the path
|
||||
# 表示路径的中心线
|
||||
# Set border cells of waypoint
|
||||
|
|
@ -543,7 +543,7 @@ class ReferencePath:
|
|||
def _compute_free_segments(self, wp, min_width):
|
||||
"""
|
||||
Compute free path segments.
|
||||
计算自由路径段。
|
||||
计算自由路径段。要求是左右边界之间上的路段是空闲的，且宽度大于最小宽度。在grid网格地图上进行搜索
|
||||
:param wp: waypoint object 路径点对象
|
||||
:param min_width: minimum width of valid segment in m 有效路径段的最小宽度（单位：米）
|
||||
:return: segment candidates as list of tuples (ub_cell, lb_cell) 作为元组列表的路径段候选项（上界网格单元，下界网格单元）
|
||||
|
|
@ -555,6 +555,7 @@ class ReferencePath:
|
|||
|
||||
# Get waypoint's border cells in map coordinates
|
||||
# 获取路径点的边界网格单元（地图坐标系）
|
||||
# 计算出参考点上的左右边界，地图的边界点坐标ud_p(x，y)表示左边界，lb_p(x，y)表示右边界
|
||||
ub_p = self.map.w2m(wp.static_border_cells[0][0],
|
||||
wp.static_border_cells[0][1])
|
||||
lb_p = self.map.w2m(wp.static_border_cells[1][0],
|
||||
|
|
@ -701,7 +702,7 @@ class ReferencePath:
|
|||
ub_ls, lb_ls = (wp.x, wp.y), (wp.x, wp.y)
|
||||
|
||||
# Check sign of upper and lower bound
|
||||
# 检查上下界的符号
|
||||
# 检查上下界的符号，根据边界点到路径点的方向角，用于判断方向
|
||||
angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x)
|
||||
- wp.psi + math.pi, 2 * math.pi) - math.pi
|
||||
angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x)
|
||||
|
|
@ -710,7 +711,7 @@ class ReferencePath:
|
|||
sign_lb = np.sign(angle_lb)
|
||||
|
||||
# Compute upper and lower bound of largest drivable area
|
||||
# 计算最大可行驶区域的上下界
|
||||
# 计算最大可行驶区域的上下界 根据上面的方向以及距离，计算上下届的数值
|
||||
ub = sign_ub * np.sqrt(
|
||||
(ub_ls[0] - wp.x) ** 2 + (ub_ls[1] - wp.y) ** 2)
|
||||
lb = sign_lb * np.sqrt(
|
||||
|
|
@ -743,7 +744,7 @@ class ReferencePath:
|
|||
angle_lb)
|
||||
bound_cells_sm = (ub_ls, lb_ls)
|
||||
# Compute cell on bound for computed distance ub and lb
|
||||
# 计算边界网格单元，用于计算距离 ub 和 lb
|
||||
# 计算不带安全距离的边界网格单元
|
||||
ub_ls = wp.x + (ub + safety_margin) * np.cos(angle_ub), wp.y + (ub + safety_margin) * np.sin(
|
||||
angle_ub)
|
||||
lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin(
|
||||
|
|
@ -752,13 +753,13 @@ class ReferencePath:
|
|||
|
||||
# Append results
|
||||
# 添加结果
|
||||
ub_hor.append(ub)
|
||||
lb_hor.append(lb)
|
||||
border_cells_hor.append(list(bound_cells))
|
||||
border_cells_hor_sm.append(list(bound_cells_sm))
|
||||
ub_hor.append(ub) # 上界，含义是距离
|
||||
lb_hor.append(lb) # 下界，含义是距离
|
||||
border_cells_hor.append(list(bound_cells)) # 不带安全距离的边界网格单元
|
||||
border_cells_hor_sm.append(list(bound_cells_sm)) # 带安全距离的边界网格单元
|
||||
|
||||
# Assign dynamic border cells to waypoints
|
||||
# 为路径点分配动态边界网格单元
|
||||
# 将带安全距离的边界网格单元分配给路径点的动态边界网格单元
|
||||
wp.dynamic_border_cells = bound_cells_sm
|
||||
|
||||
return np.array(ub_hor), np.array(lb_hor), border_cells_hor_sm
|
||||
|
|
@ -772,23 +773,28 @@ if __name__ == '__main__':
|
|||
if path == 'Sim_Track':
|
||||
|
||||
# Load map file
|
||||
# 加载地图文件
|
||||
map = Map(file_path='maps/sim_map.png', origin=[-1, -2], resolution=0.005)
|
||||
|
||||
# Specify waypoints
|
||||
# 指定路径点
|
||||
wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
|
||||
1.25, -0.75, -0.75, -0.25]
|
||||
wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
|
||||
-1.5, -1.5]
|
||||
|
||||
# Specify path resolution
|
||||
# 指定路径分辨率
|
||||
path_resolution = 0.05 # m / wp
|
||||
|
||||
# Create reference path
|
||||
# 根据参考点创建参考路径，这里计算了路径点相对于地图上的边界之间的关系，得到的参考路径的上下界，上界表示中心线左侧的距离，下界表示中心线右侧的距离（负数）
|
||||
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
|
||||
smoothing_distance=5, max_width=0.15,
|
||||
circular=True)
|
||||
circular=False)
|
||||
|
||||
# Add obstacles
|
||||
# 添加障碍物
|
||||
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
|
||||
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
|
||||
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
|
||||
|
|
@ -843,16 +849,29 @@ if __name__ == '__main__':
|
|||
print('Invalid path!')
|
||||
exit(1)
|
||||
|
||||
# Compute width of path
|
||||
# 计算路径的宽度，计算各点的上下界距离，以及边界网格单元，同时将带安全距离的网格单元赋给动态边界dynamic_border_cells
|
||||
ub, lb, border_cells = \
|
||||
reference_path.update_path_constraints(0, reference_path.n_waypoints,
|
||||
0.1, 0.01)
|
||||
|
||||
# 设置速度约束
|
||||
SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5,
|
||||
'v_min': 0, 'v_max': 1.0, 'ay_max': 4.0}
|
||||
|
||||
# Compute speed profile
|
||||
# 计算速度剖面，根据纵向加速度和侧向加速度，最大速度等条件，通过二次规划得到路径上各点的参考速度
|
||||
reference_path.compute_speed_profile(SpeedProfileConstraints)
|
||||
# Get x and y locations of border cells for upper and lower bound
|
||||
# 获取上下界的边界网格单元的 x 和 y 坐标
|
||||
# 此处似乎是多余的，在上面的代码中已经赋值，这又重复赋值
|
||||
for wp_id in range(reference_path.n_waypoints):
|
||||
reference_path.waypoints[wp_id].dynamic_border_cells = border_cells[wp_id]
|
||||
|
||||
# Display reference path
|
||||
# 显示参考路径
|
||||
reference_path.show()
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue