local_costmap（局部障碍代价地图）

local_costmap（局部障碍代价地图）

实车先看：地图上的障碍来自哪里

当前局部代价地图是以gimbal_yaw_fake位置为中心、在odom中表达的约5×5 m滚动网格。 它按顺序合成静态二维地图、实时地形障碍和障碍周围的膨胀代价，供controller_server使用。

/map二维占用图 → static_layer ──────────────────────────────┐
                                                         │
terrain_map（odom系地形点，intensity为离地高差）            │
    → 观测缓冲：TF、来源高度筛选、雷达原点                  │
    → intensity_voxel_layer：高度、高差、量程与体素筛选 ───┤
                                                         ↓
                                        inflation_layer → 局部合成代价地图
                                                         ├─ controller_server读内存中的地图
                                                         └─ costmap／costmap_updates／costmap_raw供订阅者

本块直接接收近场terrain_map；全局代价地图接收terrain_map_ext，下一块再整理。pointcloud_to_laserscan生成的obstacle_scan用于二维建图，本局部障碍层不经过那份二维扫描。因此修改扫描转换器的高差门槛，不会直接修改这里的门槛。地形分析分工、二维扫描转换

本次容器内nav2_costmap_2d、nav2_voxel_grid均为1.1.20，pb_nav2_plugins为1.0.1。本地自定义层实现与头文件和上游提交e6778ca44d9e88aa612a3c6c87d536a8cfa21705一致；以下按该版本核对。自定义层实现、体素换算与类定义

参数

下面保留发送的完整参数与原注释。实物／仿真段只有use_sim_time不同；源码缺省值及配置建议另列。

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: False
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: gimbal_yaw_fake
      rolling_window: true
      width: 5
      height: 5
      resolution: 0.05
      robot_radius: 0.2
      plugins: ["static_layer", "intensity_voxel_layer", "inflation_layer"]
      intensity_voxel_layer:
        plugin: pb_nav2_costmap_2d::IntensityVoxelLayer
        enabled: true
        track_unknown_space: true
        footprint_clearing_enabled: true
        publish_voxel_map: false
        combination_method: 1
        mark_threshold: 0
        origin_z: 0.0
        unknown_threshold: 5
        z_resolution: 0.05
        z_voxels: 16
        min_obstacle_height: 0.0
        max_obstacle_height: 2.0
        min_obstacle_intensity: 0.1
        max_obstacle_intensity: 2.0
        observation_sources: terrain_map
        terrain_map:
          data_type: PointCloud2
          # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
          # It doesn't need to start with '/'
          topic: <robot_namespace>/terrain_map
          sensor_frame: front_mid360
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.2
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.0
        inflation_radius: 0.7
      always_send_full_costmap: False

两个local_costmap层级用于匹配命名空间和节点名。例如机器人命名空间为/red_standard_robot1时，当前controller_server创建的代价地图节点为/red_standard_robot1/local_costmap/local_costmap；发布话题通常位于/red_standard_robot1/local_costmap下面。坐标系字符串不会随这个命名空间自动加前缀。

ros__parameters（更新、坐标、网格与机器人轮廓）

参数表

参数详解

update_frequency、publish_frequency与always_send_full_costmap（内部10 Hz与话题5 Hz各负责什么）

内部更新循环以WallRate运行，调用各层updateBounds、updateCosts合成地图。controller_server通过持有的local_costmap对象直接读取这张地图；它的20 Hz控制循环与本块10 Hz更新各自运行，部分控制轮次可能读到同一轮地图结果。

publish_frequency控制更新循环中是否到了发布时间，不是独立的5 Hz发布定时器；实际受更新时间、负载和ROS时钟影响。若以2.5 m/s运动，理想100 ms更新间隔内已移动0.25 m，即5个当前格距；这是时间尺度计算，不是本次实车测量。调频率需要结合整条感知和执行链延迟。更新与发布循环（1.1.20）

False不会保证只发增量：网格原点滚动、尺寸或分辨率变化时，OccupancyGrid会走全图分支；几何信息不变且有更新范围时，才可能发costmap_updates。costmap_raw不走这个增量选择，有订阅者就准备完整nav2_msgs/Costmap。使用地图话题的行为与读取服务的行为，还应各自核对数据接口。发布选择

global_frame、robot_base_frame与rolling_window（odom中的移动窗口）

网格轴按odom的x、y表达，中心跟踪gimbal_yaw_fake的平面位置，平面原点以格距对齐滚动；它不是随底盘姿态旋转的车体图。当前虚拟系的方向与速度转换关系见fake_vel_transform。

局部地图使用odom可以减少全局重定位修正直接作用到局部点云坐标的影响，但静态层仍从map系地图取样，需要有效map↔odom。只有odom至机器人TF，不证明三层合成所需TF已经全部可用。

窗口滚动只处理平面x、y。体素origin_z=0仍固定在odom的z基准上，机器人上坡、下坡不会自动平移这段垂直窗口。滚动原点实现

width、height与resolution（5×5 m，100×100格）

当前约覆盖参考点两侧各2.5 m，角落的平面距离约3.5 m；实际原点受格距对齐，不能把边界当成精确连续对称的圆。5 m观测量程用于筛点，最终落在网格外的点仍不能标记。

平面共有10000格。自定义层再为每个平面列记录16个高度状态，逻辑上约160000个体素；实现用每列32位打包，不是为每个体素存一份完整点云。减小z_voxels不等于平面列缓存内存按比例减少。

平面resolution与垂直z_resolution是独立参数，当前恰好都为0.05 m。改变平面格距还会影响障碍合并、轮廓栅格化、膨胀和控制器局部路径处理；它不能增加MID360回波密度或恢复地形分析已经过滤掉的小障碍。

robot_radius（围绕虚拟参考点的车体轮廓）

当前默认footprint=""，因此用robot_radius=0.2生成近似圆形多边形，再按默认footprint_padding≈0.01 m填充。padding按多边形顶点方向实现，不应把最终轮廓简单等同于精确半径0.21 m的圆。轮廓建立与填充

该轮廓以gimbal_yaw_fake原点为中心。需按真实底盘、雷达、云台及其活动包络核对0.2是否覆盖，而不是继承示例半径；安装参考点偏置也要计入。膨胀不能替代正确的车体轮廓。

如果以后改成车体矩形footprint，须处理其坐标轴和真实底盘姿态：当前虚拟系在平面上抵消yaw，不会自动随底盘转向。可选择覆盖转动包络的轮廓，或按实际姿态提供正确的动态轮廓，不能直接填一个底盘固连矩形就认定方向已正确。

intensity_voxel_layer（地形高差与三维障碍筛选）

参数表（工作方式与合成）

参数表（高度、体素与高差）

参数详解

高度、体素窗口与intensity（三道不同的筛选）

当前完整标记流程包括：来源缓冲高度筛选、本层高度筛选、地形高差筛选、相对传感器的三维量程筛选、平面与高度格范围，以及mark_threshold。通过其中一项不保证最终进入地图。

点的z是odom坐标；intensity是上游估计地面的高差。 假定量程、平面位置和其他条件都满足：

当前整数高度索引为floor((z−0)/0.05)，要求索引小于16，故有效范围为**[0,0.8) m**。z=0.8对应索引16，越界；上界不包含。这段窗口固定在odom，odom的z=0并不凭名称保证就是实车当前地面。

上坡高处、下坡低处的障碍即使地形高差正确，也可能被高度窗口删掉。若需要改变覆盖，必须一起核对来源高度、本层高度、origin_z与16层限制内的z_resolution；只把max_obstacle_height从2调到3不能解决体素上界。也不能为了放行坡面就把所有危险低障碍一起过滤。实际筛点与索引转换

min_obstacle_intensity与max_obstacle_intensity（是否把地形高差当障碍）

近场terrain_analysis已经把intensity改成相对估计地面的高差，当前下界0.1即10 cm。它不是“车辆能跨10 cm”，也不是坡度门槛。正常坡面若被错误估成15 cm凸起，满足其他窗口时仍可能成为障碍；应先看地面估计与坐标，再按真实车辆和场地调整门槛。

上游当前近场正常高差输出受vehicleHeight=0.5 m限制，所以这里2 m上限通常不会进一步裁剪正常点。若改输入、上游规则或出现特殊标记点，需要重新判断。直接改接原始雷达点云后，intensity可能变回反射强度，不能沿用“0.1 m／2 m”的解释。

mark_threshold（数高度格，不数原始点）

VoxelGrid用位记录每个平面列的已标记高度格；已标记格数严格大于mark_threshold才返回可标障碍。当前0表示一格就够。设1则至少要两格，设2至少三格；同一高度格里再来很多点，不会增加高度格计数。标记计数实现

本层每次更新都会先重置二维图与体素，所以门槛统计来自本轮取得的观测内容；它不会靠连续多个更新周期逐渐凑够格数。但最新terrain_map本身包含上游历史点，不能把“一份最新观测”理解成一份纯实时原始扫描。

低矮物、薄障碍、稀疏回波可能只占一个高度格，提高门槛可能让它们消失；能否过滤噪声应按实物观察，不保证越大越稳。

track_unknown_space与unknown_threshold（层级错误和未使用的门槛）

自定义层调用ObstacleLayer::onInitialize，后者读的是节点顶层track_unknown_space；Costmap2DROS及StaticLayer也读该顶层键。当前层内True没有进入这条读取路径，顶层未配置时默认False，主图和障碍层默认按FREE_SPACE重置；静态图的未知值也会按该设置转为空闲。父类读取位置、静态未知值解释

若要表达原配置“跟踪未知空间”的True意图，正确层级为：

local_costmap:
  local_costmap:
    ros__parameters:
      track_unknown_space: true

此时应移除无效的层内同名键，重新核对未知区在规划与控制中的处理。若任务选择不跟踪未知，则应明确顶层False；错误是当前写入层级，不是要求所有任务必须使用True。 这里只提供修正说明，没有修改工程配置。

unknown_threshold另属自定义成员：初始化保存5+(16−z_voxels)，当前仍为5，但后续没有用它清体素射线、统计未知列或生成二维未知状态。调整5不会改善当前未知空间判定。成员赋值与更新路径

即使顶层改True，本层也只是以未知默认二维格重置，再标出障碍和轮廓内空闲，不会因此自动得到标准VoxelLayer的完整射线空闲／未知推断。没有回波或点被过滤，不等于真实空地。

每轮重建、轮廓清空与清地图服务（几种清除各有范围）

该自定义updateBounds先resetMaps，再拿marking observations重建障碍，最后清本层轮廓内二维代价。它不调用getClearingObservations或raytraceFreespace，直接把来源clearing改True也不会给当前实现补上射线清空。

footprint_clearing_enabled=True只清本层二维轮廓，不擦除已保存的体素标记；当前Max合成也不会用FREE_SPACE覆盖static_layer的静态墙。轮廓、点云自车滤除和静态地图错误应分开核对。

清地图服务还需区分：类的isClearable=False使按区域的ClearAround／ClearExcept服务跳过它；但ClearEntirely调用resetLayers，会重置主图并调用所有层的reset，包括本层。前面的BT使用的是ClearEntireCostmap，因此不能说它因为isClearable=False就完全不重置。重置也不丢弃上游地形缓存，下一轮相同观测／静态图仍可能把障碍加回来。区域与整图服务、整图层重置

本层内部重建不保证主图所有旧格立即变空；合成还受各层更新范围、滚动、静态占用和最新观测影响。动态残留应结合上游terrain_analysis衰减与实际话题观察。

combination_method（当前Max保留已有障碍）

当前1调用updateWithMax：本层未知格跳过；已知格可以替换主图未知格，或在代价更高时写入。FREE_SPACE不会降低前面静态层的已知障碍；不能简单把255未知当作普通数字最大值来推断全部合成结果。Max合成

0为Overwrite，可用本层非未知代价覆盖之前的图层；当前顶层缺省不跟踪未知、重置大量格为FREE_SPACE，改0可能擦掉静态层的已知障碍。其他整数在当前父类switch中不做合成；新版文档中的其他枚举不代表本版本可用。

intensity_voxel_layer.terrain_map（地形观测源）

参数表

参数详解

topic、data_type与sensor_frame（点云坐标和测距原点分别取哪里）

bringup_launch先把<robot_namespace>替换为空或“/＋机器人命名空间”，随后参数重写和加载；当前配置通常订阅/red_standard_robot1/terrain_map。占位符没有在层源码中自行处理，直接启动原始参数文件时必须完成对应替换。

terrain_map的点坐标当前在odom中。ObservationBuffer按header.frame_id把点云转换到global_frame；另以sensor_frame指定的原点、同一消息时间戳取得odom中的雷达位置，再供量程计算。因此sensor_frame不会把odom点重新解释成雷达局部点。观测缓冲换系与原点

显式设置front_mid360后，消息过滤器还要求对应传感器系可变换。TF、时间域、命名空间和QoS都需要匹配；有地形话题不证明本层已成功接收缓冲。输入订阅采用SensorDataQoS，父类配置队列深度50；队列不是50 Hz或50 ms。

来源高度与量程（高度在odom，距离从雷达量）

来源高度先在转换后的odom点坐标上筛选，并保留包括intensity在内的原点字段；本层再次检查自己的高度门槛。当前两级0～2 m相同，实际可标记的上界仍受0.8 m体素覆盖制约。

量程使用sqrt((x−x_sensor)²+(y−y_sensor)²+(z−z_sensor)²)，当前严格保留0.2～5 m之间的点。与二维扫描转换器按底盘虚拟中心计算水平距离不同，雷达安装偏移、云台转动和上下坡会影响这个三维距离。

近场地形输出和局部窗口本就有限。增大量程不会扩大5×5 m网格、不能恢复上游被删的点，也不会让MID360采样更远；最小量程也不能滤掉距雷达超过0.2 m的全部自车结构。

static_layer（静态二维地图接入）

参数表

参数详解

static_layer（map图仍要经过map↔odom变换）

静态层订阅完整OccupancyGrid，保存其尺寸、分辨率、原点和header.frame_id。滚动模式不会强行把主图变成整张静态地图的大小；它根据当前odom格子的世界位置，查询map_frame←odom的最新变换，到静态图中取样。因此local_costmap在odom中运行，仍需正确的全局定位变换。滚动静态图采样

当前顶层track_unknown_space缺省False，静态图未知格转为FREE_SPACE；已知墙仍保留。没有收到地图或对应TF查询失败时，点云这一路即使存在，也不能证明静态层已正常合成。当前controller_server还会等待代价地图current状态，地图输入和各层状态要一起看。

map_subscribe_transient_local（保留地图消息与更新话题各自独立）

True让较晚启动的订阅者可以从兼容发布端取得保留的最近完整地图。若发布端只提供Volatile耐久性或可靠性不兼容，不能靠延长TF等待取得地图；应先核对发布端与订阅端的QoS。地图订阅QoS

当前subscribe_to_updates缺省False，不订阅/map_updates；这不阻止处理随后收到的完整/map消息。“static”表示图层输入方式，不表示完整地图永远不能更新。

inflation_layer（障碍周围的二维代价）

参数表

参数详解

cost_scaling_factor与inflation_radius（范围和代价下降速度）

内部代价值与发布后的OccupancyGrid占用值不同：

设d为距致命障碍格的实际平面距离、r_ins为实际填充轮廓的内切半径。障碍处为254，内圈为253，内圈之外到膨胀范围内按下面计算后取整数：

cost = 252 × exp(−cost_scaling_factor × (d − r_ins))

所以4变大时，外围代价更快降低；变小时，较高软代价维持得更远。0.7决定影响范围，4决定范围内的衰减，不能把两个参数当同一作用。代价公式（1.1.20）

仅为说明公式，假设r_ins=0.2 m、d=0.4 m：当前4对应约113，改为8约50，改为2约169。实际轮廓含padding、栅格化和最近障碍距离，不能把这个假设值当运行读数。

0.7 m以内并非全部致命或253，也不是保证车体距墙至少0.7 m。轮廓定义、真实包络、地图误差和路径使用方式都影响实际通过距离。

当前PID怎样使用这些代价（软梯度不等于已经参与避障评分）

当前OmniPidPursuitController从局部路径抽10个点，检查落格代价是否达到INSCRIBED_INFLATED_OBSTACLE，即≥253；它不按1～252的软梯度给候选轨迹打分。改变局部cost_scaling_factor或扩大外围软代价，可能改变RViz颜色，却没有直接改变本插件的碰撞通过条件。

robot_radius及padding改变的内切半径会影响253内圈；静态墙／实时障碍本身也参与高代价检查。全局路径又由另一张global_costmap规划，其膨胀设置需下一块单独核对，不能把两张地图的参数影响混为一谈。当前控制器地图使用方式

前文还记录了抽样数量及地图外点的处理缺口。这里的膨胀范围不是完整车体扫掠碰撞检查的替代，也不补回本障碍层遗漏的高度点。

相关源码默认项（原YAML未显式配置）

参数表

以下12项补充真正读取的节点顶层设置、观测缓冲与未知膨胀行为；不表示本次已向工程YAML追加。

观测停更与默认未知状态

observation_persistence=0只删除比最新观测更旧的观测，保留最后一份；expected_update_rate=0直接将缓冲判为current。于是点云停止到达时，本层可能继续每轮重建同一份旧地形，而不是自动清空障碍或停止控制。还需结合静态层、其他层状态和完整系统的数据监测判断，不能只看地图话题仍发布。缓存保留与current判断

如果以后设置正的expected_update_rate，按秒的消息间隔理解，再核对正常数据周期与延迟；不能填10就当成10 Hz。只改变buffer保留时间也不去除terrain_map内部已经累计的历史点。

动态参数、启动来源与验证范围

动态修改的实际范围

自定义层没有另写覆盖体素、高差和发布器的动态回调，使用父类ObstacleLayer的回调；因此运行中参数设置成功，不代表所有成员已经更新。

顶层transform_tolerance有成员更新分支，但已经创建的来源缓冲、消息过滤器和静态层各有初始化时保存的值；不能认为一次改值让所有TF等待同步变化。节点回调、父类层回调

启动与核对范围

• 本次已核对： 当前包版本、插件注册与liblayers.so存在；自定义层安装头文件与本地一致，本地实现与上游固定提交一致；Nav2的体素、合成与膨胀安装头文件匹配1.1.20；两份local_costmap段与安装副本一致，仿真仅时钟不同。
• 源码能确认： 40项原配置的读取／使用条件；层内未知开关位置无效、unknown_threshold未参与计算；当前odom垂直标记区间[0,0.8) m；按不同高度格计标记；每轮重建但不射线清空；区域与整图清理不同；默认旧观测复用、膨胀方向和全图／增量发布条件。
• 本次验证范围： 未启动新的地图或导航任务，未观察运行节点有效参数、坡道点、未知区、动态障碍消失时间或车旁碰撞；窗口、体素和膨胀例子是配置／源码计算。
• 实物待核实： odom地面高度基准、上下坡与高低障碍是否过窗口、真实车体轮廓与虚拟参考点、点云字段与TF时间、观察源停更处理、静态图对齐／QoS、未知区策略、动态残留以及控制器实际地图使用。