# 路网规划结合沙盘地图案例
## 1. 课程内容

>i **信息提示**
>
> *   使用到rviz可视化都在虚拟机`Rosmaster-RoadNet-Ubuntu24.04`中启动
> *  1.使用路网规划导航，结合功能沙盘地图构建城市车道路网，让车机在沙盘中导航行驶时既能遵循车道交通规则，又能规划符合路网的路径
>*   2.后续课程会有AI大模型结合沙盘自动驾驶、路网规划导航案例，这里讲解如何在沙盘中构建路网规划导航
## 2. 沙盘车道网络解析

*   这里我们可以发现，最外环车道均为**单向车道**，只能顺时针单向行驶，而中间的车道是双向车道，因此我们可以以所有的拐角和路口作为路点连接边，构建整个车道的拓扑网络

![image-20251120120000106](/media/202512/61d7f26d9a40442cb3ccecf5efee45f44089.png)

*   车道交通规则网络如下图所示

>i **信息提示**
>
> 下图中是示例的的车道交通网络，用户也可以根据自己实际需求增加更多的路点或者修改车道网络方向

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

## 3. 启动路网规划导航节点

```启动车端最大性能
sudo jetson_clocks
```
```底盘雷达驱动（车端输入代码）
ros2 launch yahboomcar_nav laser_bringup_launch.py
```

```启动可视化（虚拟机输入代码）
ros2 launch road_net_route rviz_only.launch.py
```

```打开一个容器内终端（车端输入代码）
#如果没有启动ros2_kilted docker容器，需要先启动容器
bringup_ros2kilted
#进入容器，在容器输入启动路网规划导航节点命令
exec_ros2kilted
```

```启动路网规划导航节点
ros2 launch road_net_route roadnet_nav2.launch.py map:=map.yaml pose_map:=map roadnet_file:=/root/map/map.geojson
```
可选启动参数说明
**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 |

*   将小车放置在地图中，使用`2D Pose Estimate`工具给定小车在我们建的地图上的初始位姿，大概在地图的位置，有位姿地图可以快速准确定位出车机的全局位置
![](/media/202512/79fad198cc8a46bab571eb521b5063224637.png)

## 4. 测试沙盘路网导航案例

### 4.1 路网间点到点导航

*   这里我们以10号路网点导航1号路网点为例进行演示，在终端中发布话题

>i **信息提示**
>
> 如果出发位置不在路网的点或者边上，执行路网导航的时候，机器人车机会自动先进入最近的路网边上，然后再沿路网进行以移动

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

![](/media/202512/9028425fca324e918a5f686d65687c904874.png)

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

### 4.2 任意位姿路网规划导航

*   这里我们以图中箭头方向为例进行演示
![](/media/202512/fc715f5208f444359e94f58fe792a58d4339.png)

*   先控制车机移动到这个位姿（键盘控制、手柄控制、rviz中的`2D Goal Pose`导航）

![](/media/202512/863faad41806427d82f0f9277375fcce7363.png)

*   使用如下命令查看当前机器人在地图中的坐标

```
ros2 run tf2_ros tf2_echo map base_footprint
```

*   然后让车机回到10号路点（这里是以10号路点为例，可以在地图中任意位置或路网中）
*   
![](/media/202512/926b60d9df0540fcbfc2d83cd844d5be8974.png)

*   在终端发布目标位姿话题
  
```
ros2 topic pub -1 /road_net_nav geometry_msgs/PoseStamped "{
  header: {frame_id: 'map'},
    pose: {
      position: {x: 2.648, y: -1.388, z: 0.0},
      orientation: {x: 0.0, y: 0.0, z: 1.000, w: 0.000}
  }
}"
```
车机会先沿落网抵达目标位姿最近的路点

![](/media/202512/a3bc605e473c4b48bf5079c71c34e3e25029.png)
*   然后再调整到目标位姿
![](/media/202512/10ab5bd744b7403fa2741d6cd20b40839784.png)

## 5.源码解析
*   实现程序源码路径：

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

### 3.1 route_bridge.py解析

#### 3.1.1 路点间导航

*   Route_Bridge类的初始化函数中创建了一个`road_net_nav_id`话题订阅者，接收`road_net_nav_id`话题的`Int16MultiArray`数据（格式为`[start_id, goal_id]`），直接调用`getAndTrackRoute`发起路网导航：
    
*   输入校验：若数据长度不为 2，报错误；若为`[-1, -1]`，取消导航。
    
*   导航执行：调用`self.navigator.getAndTrackRoute(start_id, goal_id)`，返回导航任务句柄。
    
*   反馈处理：循环获取导航反馈（`feedback`），监听节点切换（`last_node_id`/`next_node_id`）和重路由（`rerouted`）事件，若重路由则调用`followPath`更新局部路径。
    
*   结果处理：根据`TaskResult`（成功 / 取消 / 失败）输出日志，并通过`/action_feedback`发布结果。

```
self.road_net_nav_ID_sub= self.create_subscription(Int16MultiArray, "road_net_nav_id", self.id_nav_callback, 10)
```

```
    def id_nav_callback(self, msg:Int16MultiArray):
        ''' Callback function for navigation route messages using IDs '''
        if len(msg.data) != 2:
            self.get_logger().error(Fore.RED+"Invalid ID navigation message received."+Fore.RESET)
            return
        if msg.data[0]==-1 and msg.data[1]==-1 :
            self.navigator.cancelTask()
            self.get_logger().info(Fore.YELLOW+"navigation task canceled via message."+Fore.RESET)
            return
        route_tracking_task = self.navigator.getAndTrackRoute(msg.data[0], msg.data[1])
        task_canceled = False
        last_feedback = None
        follow_path_task = RunningTask.NONE
        while not self.navigator.isTaskComplete(task=route_tracking_task):
            feedback = self.navigator.getFeedback(task=route_tracking_task)
            while feedback is not None:
                if not last_feedback or (feedback.last_node_id != last_feedback.last_node_id or feedback.next_node_id != last_feedback.next_node_id):
                    self.get_logger().info('Passed node ' + str(feedback.last_node_id) +
                        ' to next node ' + str(feedback.next_node_id) +
                        ' along edge ' + str(feedback.current_edge_id) + '.')
                last_feedback = feedback
                if feedback.rerouted:  # or follow_path_task == RunningTask.None
                    self.get_logger().info('Passing new route to controller!')
                    follow_path_task = self.navigator.followPath(feedback.path)
                feedback = self.navigator.getFeedback(task=route_tracking_task)
            if self.navigator.isTaskComplete(task=follow_path_task):
                self.get_logger().info('Controller or waypoint follower server completed its task!')
                self.navigator.cancelTask()
                task_canceled = True
        while not self.navigator.isTaskComplete(task=follow_path_task) and not task_canceled:
            time.sleep(0.1)

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().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"))
        else:
            self.get_logger().info(Fore.RED+"navigation task failed."+Fore.RESET)
            self.action_feedback_pub.publish(String(data="road_net_nav_failed"))
```

#### 3.1.2 任意位姿导航
*   Route_Bridge类的初始化函数中创建了一个`road_net_nav`话题订阅者，接收`road_net_nav`话题的`PoseStamped`数据（格式为`[start_id, goal_id]`），直接调用`getAndTrackRoute`发起路网导航：

```
self.road_net_nav_sub= self.create_subscription(PoseStamped, "road_net_nav", self.nav_callback, 10)
```

*   `PoseStamped`：消息类型，包含目标位置）、朝向信息。
    
*   `"road_net_nav"`：话题名，通过该话题下发导航目标位姿。
    
*   `self.nav_callback`：回调函数，处理导航指令的核心逻辑。
    
*   `10`：消息队列大小，若回调处理较慢，最多缓存 10 条消息，避免消息丢失。

*   步骤 1：获取机器人当前位姿（TF 变换）：通过`tf2_ros`的`Buffer`和`TransformListener`，获取机器人基坐标系（`base_footprint`）到全局地图坐标系（`map`）的坐标变换，从而得到机器人在地图中的当前位姿。
    
*   步骤 2：构造机器人当前位姿：将 TF 变换的结果（平移 + 旋转）封装为`PoseStamped`类型的`start_pose`，作为导航的**起点位姿**。
    
*   步骤3：位姿转路网中路点 ID：这是**路网导航的核心转换逻辑**：
    
    *   调用`__find_nearest_node`方法，分别将**起点位姿**和**目标位姿**匹配到路网中最近的节点 ID（通过 KDTree 近邻搜索）。
        
    *   调用`BasicNavigator`的`getAndTrackRoute`接口，传入起点节点 ID和目标节点 ID，发起基于路网节点的导航。
        
*   步骤 4：最终姿态调整：路网导航仅保证机器人到达目标位姿附近的路点，通过`goToPose`接口让机器人精确调整至目标姿态。

```
    def nav_callback(self, msg:PoseStamped):
        ''' 任意位姿路网导航回调函数Arbitrary Pose Road Network Navigation Callback Function'''
        
        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)
            
            if self.replace2id:
                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(msg.pose.position.x, msg.pose.position.y)
                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_tracking_task = self.navigator.getAndTrackRoute(start=nearest_start_node_id,goal=nearest_goal_node_id)
            else:
                route_tracking_task = self.navigator.getAndTrackRoute(start=start_pose,goal=msg,use_start=False)
            task_canceled = False
            last_feedback = None
            follow_path_task = RunningTask.NONE
            while not self.navigator.isTaskComplete(task=route_tracking_task):
                feedback = self.navigator.getFeedback(task=route_tracking_task)
                while feedback is not None:
                    if not last_feedback or (feedback.last_node_id != last_feedback.last_node_id or feedback.next_node_id != last_feedback.next_node_id):
                        self.get_logger().info('Passed node ' + str(feedback.last_node_id) +
                            ' to next node ' + str(feedback.next_node_id) +
                            ' along edge ' + str(feedback.current_edge_id) + '.')
                    last_feedback = feedback
                    if feedback.rerouted:  # or follow_path_task == RunningTask.None
                        self.get_logger().info('Passing new route to controller!')
                        follow_path_task = self.navigator.followPath(feedback.path)
                    feedback = self.navigator.getFeedback(task=route_tracking_task)
                if self.navigator.isTaskComplete(task=follow_path_task):
                    self.get_logger().info('Controller or waypoint follower server completed its task!')
                    self.navigator.cancelTask()
                    task_canceled = True
            while not self.navigator.isTaskComplete(task=follow_path_task) and not task_canceled:
                time.sleep(0.1)
            
            result = self.navigator.getResult()
            if result == TaskResult.SUCCEEDED:
                if self.__adjust_pose(msg):
                    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"))
            else:
                self.get_logger().info(Fore.RED+"navigation task failed."+Fore.RESET)
                self.action_feedback_pub.publish(String(data="road_net_nav_failed"))
    
    def __adjust_pose(self,final_pose)->bool:
        '''调整到目标姿态Adjust to target posture'''
        self.get_logger().info("adjusting final orientation...")
        final_orientation_task = self.navigator.goToPose(final_pose)
        while not self.navigator.isTaskComplete(task=final_orientation_task):
            time.sleep(0.1)

orientation_result = self.navigator.getResult() 
        if orientation_result == TaskResult.SUCCEEDED:
            return True
        else:
            self.get_logger().info("final orientation adjustment failed.")
            return False

def __find_nearest_node(self, x, y):
        '''使用KDTree查找最近节点Use KDTree to find the nearest node'''
        distance, index = self.tree.query([x, y])
        nearest_node_id = self.node_ids[index]
        return nearest_node_id, distance
```

*   __load_roadnet_nodes方法用于在成个程序初始化阶段解析**GeoJSON 格式**的路网文件，提取路网中的`Point`类型节点（路网的拓扑节点），存储节点的坐标和 ID，并构建`KDTree`索引。

```
def __load_roadnet_nodes(self):
    '''加载路网节点用于KDTree搜索Load road network nodes for KDTree search'''
    with open(self.road_net_file, 'r') as f:
        data = json.load(f)

self.node_coords = []   # [[x, y], ...]
    self.node_ids = []      # 对应节点 ID

for feat in data['features']:
        if feat['geometry']['type'] == 'Point':
            x, y = feat['geometry']['coordinates']
            self.node_coords.append([x, y])
            self.node_ids.append(feat['properties'].get("id", None))

self.tree = KDTree(self.node_coords)
    self.get_logger().info(f"Loaded {len(self.node_coords)} nodes into KDTree")
```

### 3.2 conver_speed.py解析

*   由于ros2 kilted版本中的navigation2发布速度话题是`TwistStamped`格式，而ROSMASTER-M1底盘节点是接受Twist格式速度话题进行底盘控制，两者不能直接通信，需要一个桥梁进行速度消息格式转换

我们可以通过以下命令来查看两者消息类型的差别,两者主要差异是`std_msgs/Header header`

*   `TwistStamped`格式速度话题
```
ros2 interface show geometry_msgs/msg/TwistStamped
```
```
# A twist with reference coordinate frame and timestamp

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id
Twist twist
        Vector3  linear
                float64 x
                float64 y
                float64 z
        Vector3  angular
                float64 x
                float64 y
                float64 z
```

*   `Twist`格式速度话题

```
ros2 interface show geometry_msgs/msg/Twist
```

```
# This expresses velocity in free space broken into its linear and angular parts.

Vector3  linear
        float64 x
        float64 y
        float64 z
Vector3  angular
        float64 x
        float64 y
        float64 z
```
这里TwistStampedToTwist类的初始化函数中定义了一个TwistStamped格式的速度话题订阅者，然后callback回调函数中将TwistStamped格式的速度转换成Twist格式速度话题，并进行发布。

```
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped, Twist
from colorama import  Fore
class TwistStampedToTwist(Node):
    def __init__(self):
        super().__init__('conver_speed')
        self.sub = self.create_subscription(
            TwistStamped,
            '/cmd_vel',
            self.callback,
            5
        )
        self.pub = self.create_publisher(
            Twist,
            '/cmd_vel',
            5
        )
        self.get_logger().info(Fore.GREEN + "TwistStamped → Twist The converter has been started."+Fore.RESET)

def callback(self, msg:TwistStamped):
        t = Twist()
        t.linear = msg.twist.linear
        t.angular = msg.twist.angular
        self.pub.publish(t)

def main():
    rclpy.init()
    node = TwistStampedToTwist()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
```
## 6.导航参数调试
路网导航参数文件路径：
```
~/ros2_kilted/volumes/code_ws/src/road_net_route/config/nav2_kilted.yaml
```
### 6.1 导航控制器调试

>i **信息提示**
>
> 
> **参数调整指南**
> *   如果需要调整导航时的移动速度和转动角速度，调整 `desired_linear_vel`和`max_angular_accel`   
> *   如果需要调整抵达目标点的精度，调整`xy_goal_tolerance`和`yaw_goal_tolerance`  
> *   其他参数调整详见下方注释

```
controller_server:
  ros__parameters:
    # ===================== 控制器核心全局参数 =====================
    # 控制器的主循环频率（Hz），决定局部路径规划和速度指令的发布频率，需与硬件执行频率匹配
    controller_frequency: 20.0
    # 代价地图更新超时时间（秒），若代价地图在该时间内未更新，控制器将停止输出速度指令，避免基于过期地图规划
    costmap_update_timeout: 0.30
    # X方向（前进/后退）最小速度阈值（米/秒），低于该值的速度指令会被判定为无效，用于过滤微小抖动
    min_x_velocity_threshold: 0.001
    # Y方向（横移）最小速度阈值（米/秒），仅适用于全向移动机器人（如麦克纳姆轮），差分驱动机器人可设为较大值（如0.5）禁用横移
    min_y_velocity_threshold: 0.5
    # 旋转方向最小速度阈值（弧度/秒），低于该值的旋转指令会被判定为无效，过滤微小旋转抖动
    min_theta_velocity_threshold: 0.001
    # 控制器失败容忍度（米），当局部规划的路径偏差超过该值时，判定控制器执行失败，触发重规划
    failure_tolerance: 0.3
    # 进度检查器插件列表，用于监控机器人是否向目标移动
    progress_checker_plugins: ["progress_checker"]
    # 目标检查器插件列表，用于判断机器人是否到达目标位姿
    goal_checker_plugins: ["general_goal_checker"] 
    # 控制器插件列表，核心是局部路径跟踪算法，这里使用FollowPath对应的纯追踪控制器
    controller_plugins: ["FollowPath"]
    # 是否使用实时优先级调度控制器线程
    use_realtime_priority: false
    # 速度限制话题名称，控制器会订阅该话题的速度指令，动态调整最大线速度/角速度（如避障时降速）
    speed_limit_topic: "speed_limit"

# ===================== 进度检查器参数（SimpleProgressChecker） =====================
    progress_checker:
      # 进度检查器插件类型，SimpleProgressChecker为基础实现，监控机器人移动距离和时间
      plugin: "nav2_controller::SimpleProgressChecker"
      # 机器人需要移动的最小距离阈值（米），在movement_time_allowance时间内未移动超过该距离，判定为无进度
      required_movement_radius: 0.05 
      # 允许的最大无进度时间（秒），超过该时间仍未满足required_movement_radius，控制器将中止任务（错误码105）
      movement_time_allowance: 10.0

# ===================== 目标检查器参数（SimpleGoalChecker） =====================
    general_goal_checker:
      # 是否为有状态检查器，设为True时会持续监控目标位姿，直到满足容差；False则仅单次检查
      stateful: True
      # 目标检查器插件类型，SimpleGoalChecker为基础实现，判断位置和角度是否在容差范围内
      plugin: "nav2_controller::SimpleGoalChecker"
      # XY平面位置容差（米），机器人位置与目标位置的偏差小于该值时，判定位置到达
      xy_goal_tolerance: 0.15
      # 航向角容差（弧度），机器人朝向与目标朝向的偏差小于该值时，判定角度到达
      yaw_goal_tolerance: 0.20

# ===================== 纯追踪控制器参数（RegulatedPurePursuitController） =====================
    FollowPath:
      # 局部路径跟踪插件类型，RegulatedPurePursuitController为带速度调节的纯追踪算法，适合差分驱动机器人
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      # 期望线性速度（米/秒），机器人正常行驶时的目标线速度，会被避障/曲率调节动态缩放
      desired_linear_vel: 0.16
      # 前瞻距离（米），纯追踪算法的核心参数，决定机器人追踪的路径点距离，需与机器人速度匹配
      lookahead_dist: 0.5
      # 最小前瞻距离（米），防止前瞻距离过小导致机器人频繁转弯
      min_lookahead_dist: 0.05
      # 最大前瞻距离（米），防止前瞻距离过大导致机器人反应迟钝
      max_lookahead_dist: 0.9
      # 前瞻时间（秒），若启用use_velocity_scaled_lookahead_dist，前瞻距离=desired_linear_vel * lookahead_time
      lookahead_time: 1.5
      # 旋转到目标朝向的角速度（弧度/秒），机器人到达位置后，调整朝向的旋转速度
      rotate_to_heading_angular_vel: 1.3
      # TF变换容忍度（秒），允许机器人位姿变换的时间戳与当前时间的最大偏差，解决TF延迟问题
      transform_tolerance: 0.5
      # 是否使用速度缩放的前瞻距离，设为True时，前瞻距离随机器人速度动态变化（速度越快，前瞻越远）
      use_velocity_scaled_lookahead_dist: false
      # 接近目标时的最小线速度（米/秒），机器人距离目标较近时，降速到该值，避免冲过目标
      min_approach_linear_velocity: 0.05
      # 接近速度缩放距离（米），机器人距离目标小于该值时，开始线性降速（从desired_linear_vel到min_approach_linear_velocity）
      approach_velocity_scaling_dist: 1.0
      # 是否启用碰撞检测，设为True时，控制器会检查前瞻路径上的碰撞，避免机器人撞障
      use_collision_detection: false
      # 到前瞻点的最大允许碰撞时间（秒），仅当use_collision_detection为True时生效，用于判断碰撞风险
      max_allowed_time_to_collision_up_to_carrot: 1.0
      # 是否启用基于路径曲率的线速度调节，设为True时，转弯半径越小，线速度越低
      use_regulated_linear_velocity_scaling: true
      # 是否启用基于代价地图的线速度调节，设为True时，障碍物越近，线速度越低
      use_cost_regulated_linear_velocity_scaling: false
      # 触发曲率调节的最小转弯半径（米），当路径曲率对应的转弯半径小于该值时，开始降速
      regulated_linear_scaling_min_radius: 0.05   
      # 曲率调节的最小速度（米/秒），即使转弯半径极小，线速度也不会低于该值
      regulated_linear_scaling_min_speed: 0.25
      # 是否使用固定曲率的前瞻距离，设为True时，前瞻距离由curvature_lookahead_dist决定
      use_fixed_curvature_lookahead: false
      # 曲率前瞻距离（米），仅当use_fixed_curvature_lookahead为True时生效
      curvature_lookahead_dist: 1.0
      # 是否启用“先旋转到朝向再移动”，设为True时，若机器人朝向与路径方向偏差过大，先旋转再前进
      use_rotate_to_heading: true
      # 触发旋转到朝向的最小角度（弧度），机器人朝向与路径方向偏差超过该值时，开始旋转调整
      rotate_to_heading_min_angle: 0.785 # 约45度
      # 最大角加速度（弧度/秒²），限制机器人旋转的加速度，避免急转导致硬件损坏
      max_angular_accel: 2.7
      # 机器人位姿搜索的最大距离（米），当控制器无法在路径上找到匹配的机器人位姿时，搜索该距离内的路径点
      max_robot_pose_search_dist: 10.0
      # 目标点后是否插值曲率，设为True时，目标点后的路径会插值曲率，避免机器人到达目标后突然停止
      interpolate_curvature_after_goal: false
      # 代价缩放距离（米），仅当use_cost_regulated_linear_velocity_scaling为True时生效，用于计算避障降速的权重
      cost_scaling_dist: 0.08
      # 代价缩放增益，仅当use_cost_regulated_linear_velocity_scaling为True时生效，增益越大，避障降速越明显
      cost_scaling_gain: 1.0
      # 膨胀代价缩放因子，用于将代价地图的膨胀层代价转换为速度缩放系数，数值越大，对膨胀区的避障越敏感
      inflation_cost_scaling_factor: 3.0
```
### 6.2 避障和代价地图调试

>i **信息提示**
>
> 如果机器人导航时太靠近地图中障碍物，可以通过调整膨胀区域让机器人远离障碍物，增大local_costmap和global_costmap中的inflation_radius（膨胀半径）、cost_scaling_factor（缩放因子，值越大，越不容易靠经障碍物）其他参数调整详见下方注释

```
# 局部代价地图配置：用于机器人实时避障的短期代价地图，随机器人移动滚动更新
local_costmap:
  local_costmap:
    ros__parameters:
      # ===================== 代价地图核心全局参数 =====================
      # 代价地图更新频率（Hz），决定局部地图的刷新速度，需与传感器频率匹配
      update_frequency: 5.0
      # 代价地图发布频率（Hz），向RViz等可视化工具发布地图的频率，低于更新频率可减少带宽占用
      publish_frequency: 2.0
      # 代价地图的全局坐标系，局部地图使用odom系
      global_frame: odom
      # 机器人基坐标系，与硬件配置的机器人中心坐标系一致
      robot_base_frame: base_link
      # 是否启用滚动窗口模式，局部地图的核心特性：地图随机器人移动而滚动，仅保留机器人周围区域的地图
      rolling_window: true
      # 滚动窗口的宽度（米），局部地图的横向范围
      width: 3
      # 滚动窗口的高度（米），局部地图的纵向范围
      height: 3
      # 代价地图的分辨率（米/像素），表示每个像素代表的实际距离，0.05即5厘米/像素
      resolution: 0.05
      # 机器人轮廓（多边形），用坐标列表表示机器人的实际形状
      footprint: "[[0.14,0.11], [0.14,-0.11], [-0.14,-0.11], [-0.14,0.11]]"
      # 机器人轮廓的填充距离（米），在实际轮廓外额外扩展的安全距离，用于避障时的缓冲
      footprint_padding: 0.0616
      # 代价地图的图层插件列表，按顺序加载（voxel_layer：体素障碍层；inflation_layer：膨胀层）
      plugins: ["voxel_layer", "inflation_layer"]
      # 代价地图的过滤器插件列表，用于对地图进行后处理（keepout_filter：禁区过滤）
      filters: ["keepout_filter"]
      # 是否始终发布完整的代价地图，设为True时每次更新都会发布全图，False则仅发布变化区域（减少带宽）
      always_send_full_costmap: True
      # 服务内省模式，disabled表示禁用服务内省，可选enabled/verbose
      service_introspection_mode: "disabled"

# ===================== 过滤器插件：禁区过滤（KeepoutFilter） =====================
      keepout_filter:
        # 过滤器插件类型，KeepoutFilter用于在代价地图中标记禁区
        plugin: "nav2_costmap_2d::KeepoutFilter"
        # 是否启用禁区过滤，KEEPOUT_ZONE_ENABLED为外部参数（需在启动时赋值true/false）
        enabled: KEEPOUT_ZONE_ENABLED
        # 禁区信息的订阅话题，接收禁区的几何形状和位置信息（如PolygonStamped）
        filter_info_topic: "keepout_costmap_filter_info"
        # 是否覆盖禁区的致命代价，设为True时用自定义代价替代默认的致命代价（255）
        override_lethal_cost: True
        # 自定义的禁区致命代价，200表示比默认致命代价（255）低，但仍为高代价禁区
        lethal_override_cost: 200

# ===================== 图层插件：膨胀层（InflationLayer） =====================
      inflation_layer:
        # 图层插件类型，InflationLayer用于在障碍周围生成膨胀区，避免机器人碰撞
        plugin: "nav2_costmap_2d::InflationLayer"
        # 代价缩放因子，控制膨胀区的代价衰减速度（数值越大，代价衰减越快，膨胀区越小）
        cost_scaling_factor: 3.0
        # 膨胀半径（米），障碍周围的膨胀范围，机器人轮廓到障碍的最小安全距离
        inflation_radius: 0.1

# 全局代价地图配置：用于机器人全局路径规划的长期代价地图，覆盖整个环境
global_costmap:
  global_costmap:
    ros__parameters:
      # ===================== 代价地图核心全局参数 =====================
      # 代价地图更新频率（Hz），全局地图无需高频更新，设为1-2Hz即可
      update_frequency: 1.0
      # 代价地图发布频率（Hz），与更新频率一致即可
      publish_frequency: 1.0
      # 代价地图的全局坐标系，全局地图使用map系（全局一致，适合路径规划）
      global_frame: map
      # 机器人基坐标系，与局部地图保持一致
      robot_base_frame: base_link
      # 机器人半径（被footprint替代，注释掉）
      # robot_radius: 0.22
      # 机器人轮廓（与局部地图一致）
      footprint: "[[0.14,0.11], [0.14,-0.11], [-0.14,-0.11], [-0.14,0.11]]"
      # 机器人轮廓的填充距离（与局部地图一致）
      footprint_padding: 0.0616
      # 代价地图的分辨率（与局部地图一致，保证坐标匹配）
      resolution: 0.05
      # 是否跟踪未知空间，设为True时地图中标记未探索的区域（用于SLAM和未知环境规划）
      track_unknown_space: true
      # 代价地图的图层插件列表（static_layer：静态层；obstacle_layer：2D障碍层；inflation_layer：膨胀层）
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      # 代价地图的过滤器插件列表（keepout_filter：禁区过滤；speed_filter：速度限制过滤）
      filters: ["keepout_filter", "speed_filter"]
      # 是否始终发布完整的代价地图
      always_send_full_costmap: True
      # 服务内省模式
      service_introspection_mode: "disabled"
   
      inflation_layer:
        # 图层插件类型，InflationLayer用于在障碍周围生成膨胀区，避免机器人碰撞
        plugin: "nav2_costmap_2d::InflationLayer"
        # 代价缩放因子，控制膨胀区的代价衰减速度（数值越大，代价衰减越快，膨胀区越小）
        cost_scaling_factor: 3.0
        # 膨胀半径（米），障碍周围的膨胀范围，机器人轮廓到障碍的最小安全距离
        inflation_radius: 0.1
```