diff --git a/src/reference_path.py b/src/reference_path.py index 299713d..c6d667a 100644 --- a/src/reference_path.py +++ b/src/reference_path.py @@ -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()