## 1. 课程内容

>i **信息提示**
>
> 使用到rviz可视化都在虚拟机`Rosmaster-RoadNet-Ubuntu24.04`中启动
> 基础
> 运行示例程序，在路网规划导航的基础上，如果行驶过程中检测到行驶的路径上被阻> 塞，则会进入重规划在路网中寻找新的路径
>  进阶
> 1.理解重规划的实现方法，以及如何更改重规划策略（策略不唯一，可以根据需求自行实现）
> 2.根据查询的代价值调试更改障碍物检测阈值

## 2 编辑路网
*   为了演示重规划功能，我们在新的路网中添加一些边，打开和编辑路网的操作教程参考：**03-路网标注**的第4小节：载入和修改路网描述
![image-20251127193445946](/media/202512/16acb8f942924ee0b04537bc311663866296.png)

*   我们在路点1和6之间添加双向边
![image-20251127201258423](/media/202512/bc4279062bec4cb8a81df539a4bfa2ed7102.png)

*   所有的操作在终端中都会有对应的记录

![image-20251127201344877](/media/202512/37c3afc5402e41d691e523a49b698e8a2265.png)

*   之后保存文件
![image-20251127201843407](/media/202512/2a52c9401c494be080939283b6c95be21392.png)

## 2. 运行重规划案例
### 2.1 启动路网规划导航节点

```（主机端）底盘雷达驱动
ros2 launch yahboomcar_nav laser_bringup_launch.py
```
```（虚拟机端）启动可视化
ros2 launch road_net_route rviz_only.launch.py
```
**（主机端）打开一个容器内终端,如果没有启动ros2_kilted docker容器,需要先启动容器**
```
bringup_ros2kilted
```
```进入容器
exec_ros2kilted
```

![](/media/202512/f2ae62ac84864f729c03027921bedbbb7905.png)

```启动路网规划导航节点
ros2 launch road_net_route roadnet_nav2.launch.py map:=map.yaml pose_map:=map roadnet_file:=/root/map/map.geojson
```

>i **信息提示**
>
> 
可选启动参数说明

**map** 需要加载的栅格地图文件名称，默认值为`map.yaml`

**pose_map** 需要加载的位姿地图文件，当导航全局定位方式使用slam\_toolbox纯定位时，必须传入位姿地图

**roadnet_file** 路网描述文件，需要是完整文件路径

**params_file** 导航参数文件，默认值为`os.path.join(get_package_share_directory('road_net_route'), 'config', 'nav2_kilted.yaml')`

**localization_mode** 全局定位方式，默认为slam_toolbox

| 参数名          | 输入       | 最终真实路径                |
|--------------|----------|-----------------------|
| map          | map.yaml | /root/map/map.yaml    |
| pose_map     | map      | /root/map/map         |
| roadnet_file | 绝对路径     | /root/map/map.geojson |

![image-20251127202710855](/media/202512/43f8dfae37ef4a1386a019fa476559867779.png)

*   这里测试场景为：从路点8沿路网行进至路点6,然后在路网规划好的路径上放上障碍物,触发重规划策略

ros2 topic pub -1 /road_net_nav_id std_msgs/msg/Int16MultiArray "{data: [8, 6]}"

![image-20251127205025067](/media/202512/51fb67287ce44ca8923f4ce7907c73916164.png)

*   这里可以看到终端会提示路径上存在障碍物，然后触发重规划![image-20251127205611749](/media/202512/7b750c44427c4b72ac4ae94fd97ba00a5974.png)

*   这里检测到路径上存在上障碍物，路径不可通行后，退回到上个经过的路点8

![image-20251127205823822](/media/202512/784e3a838bd541589b86a7097339d20e6664.png)

*   然后从路点8重新尝试规划可行驶的路径，然后抵达目标路点6

![image-20251127205147761](/media/202512/7fe29aa01bfd4d2d871125fb6358365e5843.png)

## 3. 查询障碍物代价阈值

*   这里检测路径上的障碍物是通过检测全局路径上的代价值和全局路径中高代价值的点数决定的，触发重规划的阈值是可以自己设定的

>i **信息提示**
>
> 
*   检测障碍物阈值一般按出厂默认即可！！！，如果有特殊场景的需求，可以参考这部分教程调整触发重规划的阈值 
*   代价值是代价地图中用来描述障碍物占据范围的值，越靠近障碍物的地方，代价值越高。
*   
### 3.1 查看障碍物代价值

*   点击Publish Point工具，然后地图上我们想查看地图代价值的地方使用鼠标左键点击一下，即可查看指定位置的代价值

![image-20251127211505111](/media/202512/ec09d50872b64e9ea76eb00247d3c6b57068.png)

*   可以看到没有障碍物 的地方代价值为0,障碍物中心代价值 为99-100

![image-20251127212112982](/media/202512/e9b23c39963341babbe8bbdc260b465b1378.png)

### 3.2 修改触发重规划的阈值

*   配置文件路径

~/ros2_kilted/volumes/code_ws/src/road_net_route/config/nav2_kilted.yaml

*   找到配置文件中的route_bridge节点配置参数，其中`COST_THRESHOLD`和`PATH_OBSTACLES_THRESHOLD`共同决定触发的阈值‘

```
route_bridge:
  ros__parameters:
    roadnet_file: '/root/map/map.geojson'
    COST_THRESHOLD: 60
    replace2id: True
    PATH_OBSTACLES_THRESHOLD: 5
```

*   具体检测障碍物逻辑如下
    
*   路网功能规划的全局路径是ros中`nav_msgs/msg/Path`消息类型，是由多个位姿点组成的路径，在进行路网导航时，会检测全局路径上所有的位姿点是否在全局代价地图中存在高代价值的点。
    
    *   如果超过**COST_THRESHOLD**阈值，就标记路径上的该点为高代价值位姿点
        
    *   最后计算出全局上有多少个位姿点是代价值位姿点
        
    *   如果高代价值的位姿点总数大于**PATH_OBSTACLES_THRESHOLD**，则认为这条路径不可通行，需要进行重规划

Tip

如果检测障碍物不灵敏，可以减小**COST_THRESHOLD**和**PATH_OBSTACLES_THRESHOLD**，如果过于灵敏，可以减小这两个值或者调整障碍物的膨胀半径（修改障碍物膨胀半径参考04-路网间规划导航的4.1 导航控制器调试）

![image-20251127214205383](/media/202512/490e566d82b44efc813fe97912cf0b1c7191.png)

## 4. 源码解析

*   实现程序源码路径：

~/ros2_kilted/volumes/code_ws/src/road_net_route/road_net_route/route_bridge.py

### 4.1 障碍物检测和重规划策略

*   遍历全局路径的每个位姿点，通过全局代价地图检查路径上是否存在超过阈值的障碍物，返回布尔值表示路径是否被障碍物阻挡。
    
*   **世界坐标转地图坐标**：调用代价地图的`worldToMapValidated`方法，将世界坐标系的`(wx, wy)`转换为代价地图的**栅格坐标**`(mx, my)`（代价地图是二维栅格地图，每个栅格对应一个代价值）。
    
*   **坐标有效性检查**：如果转换后的栅格坐标为`None`，说明该路径点超出了全局代价地图的范围，打印警告日志并跳过该点。
    
*   **参数**：`path:Path` → ROS2 的`nav_msgs/msg/Path`消息类型，存储机器人的全局规划路径，包含一系列位姿（`poses`）。
    
*   **返回值**：`bool` → `True`表示路径上障碍物数量超过阈值，`False`表示路径可行。

*   
```
def __check_path_obstacles(self, path:Path)->bool:
    """检查全局路径上是否存在障碍物Check for obstacles on the global path."""
    pose_array=[]
    for pose in path.poses:
        wx=pose.pose.position.x
        wy=pose.pose.position.y

mx, my = self.global_costmap.worldToMapValidated(wx,wy )
        if mx is None or my is None:
            self.get_logger().warn(f"Path point ({wx}, {wy}) is outside global costmap")
            continue

cost = self.global_costmap.getCostXY(int(mx),int(my))

if cost > self.COST_THRESHOLD:
            pose_array.append(pose)
    if len(pose_array) >self.PATH_OBSTACLES_THRESHOLD:
        self.get_logger().info(f"Path contains {len(pose_array)} potential obstacles.")
        return True
    return False
```

*   当全局路径检测到障碍物后，让机器人**回退到上一个路点**，并重新规划路径。
*   
```
def __reroute_strategy_1(self,last_id):
    '''重规划策略1:回退到上一个路点,重新规划可行驶路径Replanning Strategy 1: Go back to the previous waypoint and replan the drivable path.'''
    navigator= BasicNavigator()
    goal_x,goal_y=self.__get_node_coordinates(last_id)
    if goal_x is not None and goal_y is not None:
        goal_pose = PoseStamped()
        goal_pose.header.frame_id = "map"
        goal_pose.pose.position.x = goal_x
        goal_pose.pose.position.y = goal_y
        back_lastid = navigator.goToPose(goal_pose)
        while not navigator.isTaskComplete(task=back_lastid):
            time.sleep(0.1)
        if navigator.getResult() == TaskResult.SUCCEEDED:
            self.get_logger().info("back to last id  success.")
        else:
            self.get_logger().info("back to last id  failed.")
```

*   handle_id_navigation和handle_pose_navigation是负责路网规划导航的核心代码，其中两个函数中的以下部分，是检测路径和触发重规划的逻辑
    
    当检测到路径不可行时，调用**重规划策略 1**：让机器人**回退到上一个路点**（`last_node_id`对应的坐标）。该方法的核心逻辑已在之前讲解过：初始化导航器→获取`last_node_id`的坐标→发送 “回退到该路点” 的导航任务→等待任务完成并打印结果。 将机器人从当前的障碍物区域退回到**安全的预定义路点**，为后续重新规划路径提供一个稳定的起点。
    
    完整流程如下：
    
    1.  机器人正常执行路径跟踪，实时获取导航反馈`feedback`。
        
    2.  检测`feedback`中的路径是否存在障碍物：
        
        *   若无障碍物：继续执行原路径，无需处理。
            
        *   若有障碍物：触发重规划逻辑。
            
    3.  机器人回退到上一个安全路点（`last_node_id`）。
        
    4.  以该路点为起点，重新规划到目标路点（`goal_id`）的路径。
        
    5.  获取新路径的反馈，让机器人跟踪新路径继续移动。

```
if feedback is not None and self.__check_path_obstacles(feedback.path):
    self.__reroute_strategy_1(last_node_id)
    route_task = self.navigator.getAndTrackRoute(last_node_id, goal_id)
    feedback = self.navigator.getFeedback(task=route_task)
    follow_path_task = self.navigator.followPath(feedback.path)
```

```
def handle_id_navigation(self, start_id:int, goal_id:int):
        '''路点间导航road network ID navigation'''
        
        route_task = self.navigator.getAndTrackRoute(start_id, goal_id)
        feedback=None
        follow_path_task=RunningTask.NONE
        last_feedback = None
        last_node_id =999 
        while not self.navigator.isTaskComplete(task=route_task):
            feedback = self.navigator.getFeedback(task=route_task)
            if feedback is not None and self.__check_path_obstacles(feedback.path):
                self.__reroute_strategy_1(last_node_id)
                route_task = self.navigator.getAndTrackRoute(last_node_id, goal_id)
                feedback = self.navigator.getFeedback(task=route_task)
                follow_path_task = self.navigator.followPath(feedback.path)
            while feedback is not None:
                if  last_feedback is not None and (feedback.last_node_id != last_feedback.last_node_id or feedback.next_node_id != last_feedback.next_node_id):
                    self.get_logger().info(f'id:{feedback.last_node_id} ------> id:{feedback.next_node_id} edge:{feedback.current_edge_id}')
                last_feedback = feedback
                last_node_id=feedback.last_node_id
                if feedback.rerouted : 
                    self.get_logger().info('reroute a new path to follow!')
                    follow_path_task = self.navigator.followPath(feedback.path)
                feedback = self.navigator.getFeedback(task=route_task)

if self.navigator.isTaskComplete(task=follow_path_task):
                print(follow_path_task)
                self.get_logger().info('Controller completed its task!')
                if follow_path_task != RunningTask.NONE :
                    self.navigator.cancelTask()

result = self.navigator.getResult()
        if result == TaskResult.SUCCEEDED:
            self.get_logger().info(Fore.GREEN+"navigation task completed."+Fore.RESET)
            self.action_feedback_pub.publish(String(data="road_net_nav_succeeded"))
        elif result == TaskResult.CANCELED:
            self.get_logger().warn("Navigation task canceled.")
        elif result == TaskResult.FAILED:
            self.get_logger().error("Navigation task failed.")
            self.action_feedback_pub.publish(String(data="road_net_nav_failed"))

def handle_pose_navigation(self, goal_pose:PoseStamped):
        '''处理任意位姿导航 arbitrary pose navigation'''
        transform = self.tf_buffer.lookup_transform("map", "base_footprint", rclpy.time.Time(),rclpy.duration.Duration(seconds=5.0))
        if transform is None:
            self.get_logger().info(Fore.RED+"Failed to get TF transform "+Fore.RESET)
            return

else:
            start_pose = PoseStamped()
            start_pose.header = transform.header
            start_pose.pose.position.x = transform.transform.translation.x
            start_pose.pose.position.y = transform.transform.translation.y
            start_pose.pose.orientation = transform.transform.rotation
            self.get_logger().info(Fore.CYAN + f"current:{transform.transform.translation}"+Fore.RESET)
            nearest_start_node_id, nearest_start_distance = self.__find_nearest_node(start_pose.pose.position.x, start_pose.pose.position.y)
            nearest_goal_node_id, nearest_goal_distance = self.__find_nearest_node(goal_pose.pose.position.x, goal_pose.pose.position.y)
            if self.replace2id:
                self.get_logger().info(Fore.CYAN + f"nearest_start_node_id:{nearest_start_node_id}, distance:{nearest_start_distance}"
                                    f"\nnearest_goal_node_id:{nearest_goal_node_id}, distance:{nearest_goal_distance}"+Fore.RESET)
                route_task = self.navigator.getAndTrackRoute(start=nearest_start_node_id,goal=nearest_goal_node_id)
            else:
                route_task = self.navigator.getAndTrackRoute(start=start_pose,goal=goal_pose,use_start=True)
                
            feedback=None
            follow_path_task=RunningTask.NONE
            last_feedback = None
            last_node_id =999 
            while not self.navigator.isTaskComplete(task=route_task):
                feedback = self.navigator.getFeedback(task=route_task)
                if feedback is not None and self.__check_path_obstacles(feedback.path):
                    self.__reroute_strategy_1(last_node_id)
                    route_task = self.navigator.getAndTrackRoute(last_node_id, nearest_goal_node_id)
                    feedback = self.navigator.getFeedback(task=route_task)
                    follow_path_task = self.navigator.followPath(feedback.path)
                while feedback is not None:
                    if  last_feedback is not None and (feedback.last_node_id != last_feedback.last_node_id or feedback.next_node_id != last_feedback.next_node_id):
                        self.get_logger().info(f'id:{feedback.last_node_id} ------> id:{feedback.next_node_id} edge:{feedback.current_edge_id}')
                    last_feedback = feedback
                    last_node_id=feedback.last_node_id
                    if feedback.rerouted :  
                        self.get_logger().info('reroute a new path to follow!')
                        follow_path_task = self.navigator.followPath(feedback.path)
                    feedback = self.navigator.getFeedback(task=route_task)

if self.navigator.isTaskComplete(task=follow_path_task):
                    self.get_logger().info('Controller completed its task!')
                    if follow_path_task != RunningTask.NONE :
                        self.navigator.cancelTask()

result = self.navigator.getResult()
            if result == TaskResult.SUCCEEDED:
                if self.__adjust_pose(goal_pose):
                    self.get_logger().info(Fore.GREEN+"navigation task completed."+Fore.RESET)
                    self.action_feedback_pub.publish(String(data="road_net_nav_succeeded"))

elif result == TaskResult.CANCELED:
                self.get_logger().info(Fore.YELLOW+"navigation task canceled."+Fore.RESET)

elif result == TaskResult.FAILED:
                self.get_logger().info(Fore.RED+"navigation task failed."+Fore.RESET)
                self.action_feedback_pub.publish(String(data="road_net_nav_failed"))
```

### 4.2 查询地图代价值实现方法

*   调用全局代价地图的`worldToMapValidated`方法，将**世界坐标`(x, y)`转换为代价地图的栅格坐标`(mx, my)`**。
    
*   代价地图是**二维栅格地图**（每个栅格对应现实世界中的一个小区域， 0.05m×0.05m），`worldToMapValidated`是安全的转换方法：若坐标超出代价地图范围，返回`None`，避免直接转换导致的越界错误。
    
*   **获取代价值**：调用代价地图的`getCostXY`方法，传入栅格坐标的整数形式（栅格坐标是离散的整数，避免浮点型传入导致的错误），获取该栅格的**代价值`cost`**。

```
    def get_costmap_value(self, msg:PointStamped):
        '''在rviz中通过点击查看某个点在全局代价地图的代价值In RViz, you can click to view the cost of a point on the global cost map.'''
        x=msg.point.x
        y=msg.point.y
        mx, my = self.global_costmap.worldToMapValidated(x, y)
        if mx is None or my is None:
            self.get_logger().warn(f"Path point ({x}, {y}) is outside local costmap")
            return
        cost = self.global_costmap.getCostXY(int(mx),int(my))
        self.get_logger().info(f"clicked_point in global_costmap cost:{cost}")
```

路径重规划