terrain_analysis_ext（扩展远场地形与近远场结果合并）

1.terrain_analysis_ext（扩展远场地形与近远场结果合并）

1.1.实车先看：坡道、落差与近远场衔接

This node provides terrain points to the global costmap.Within the current 4 m, it uses the result of terrain_analysis; only beyond 4 m does it output its own computed result. Therefore, changing this block's ground estimation or height filtering parameters will not simultaneously change the near field; 4 m is the handover distance between the two results, not the radar's maximum range.

These parameters help the system see the terrain clearly, but they cannot give the slope or step height permitted for an actual vehicle. The vehicle's climbing ability also depends on the chassis, center of gravity, traction, and actual measurements; currently there is no ramp or physical verification.

1.2.参数

1.2.1.ros__parameters (ROS node parameters)

1.2.1.1.Parameter table

terrain_analysis_ext:
  ros__parameters:
    scanVoxelSize: 0.1
    decayTime: 0.2
    noDecayDis: 0.0
    clearingDis: 20.0
    useSorting: true
    quantileZ: 0.2
    vehicleHeight: 1.0
    voxelPointUpdateThre: 100
    voxelTimeUpdateThre: 2.0
    lowerBoundZ: -1.5
    upperBoundZ: 0.5
    disRatioZ: 0.2
    checkTerrainConn: False
    terrainUnderVehicle: -0.75
    terrainConnThre: 0.5
    ceilingFilteringThre: 2.0
    localTerrainMapRadius: 4.0

1.2.1.2.Parameter Details

1.2.1.2.1.Input, output, and altitude datum

The x, y, z of its own far-field retained points; the points are not moved onto the ground; what is rewritten is intensity, which is no longer radar reflection intensity. The current global costmap's IntensityVoxelLayer uses this output, and filters height differences with min_obstacle_intensity=0.1 and max_obstacle_intensity=2.0. Reaching 10 cm only satisfies one of the conditions; it must also pass filtering by coordinate height, distance, etc., before it can possibly become an obstacle.

Below, above or below the LiDAR means a point’s odom z minus the input pose z; distance means the horizontal distance in odom . The code does not query LiDAR→chassis TF here or redefine vertical direction using gimbal orientation. Upstream loam_interface treats the input as a LiDAR pose, whereas raw Point-LIO output describes the internal IMU. The previously recorded missing IMU→LiDAR conversion still affects this section’s height reference. Widening the height range cannot replace fixing the interface.

1.2.1.2.2.lowerBoundZ, upperBoundZ, and disRatioZ: whether distant uphill and downhill slopes can be seen

These three items first determine whether a slope point is eligible to remain, before discussing whether the slope will be treated as an obstacle. The current upper and lower boundaries are relative to the input pose and expand with distance:

For example, a downhill point 5 m away that is 2.0 m lower than the input origin can pass the current window; if it is 3.0 m lower, it will be deleted. An uphill point 5 m away that is 1.0 m higher can pass; if it is 2.0 m higher, it will be deleted. This only illustrates height filtering; the subsequent output conditions must still be satisfied.

Therefore:If the downhill ground is missing, check the lower bound; if the uphill ground is missing, check the upper bound; the farther away it is, the more it is missing, so check together with the distance relaxation ratio. "Increasing" the lower bound means the value becomes larger, for example from -1.5 to -1.0, which instead narrows the downward retention range.

Unlike the previous block, the expanded node uses this distance-expanding window when entering the cache, updating the cache, selecting the ground, and outputting its own far-field; it does not revert to fixed upper and lower bounds at the final output.disRatioZ=0.2 This means that for each additional 1 m of horizontal distance, the upper and lower bounds are each relaxed by 0.2 m; it does not represent the allowed slope or the slope surface recognition result.

Before installing the physical object, first verify the origin height above the ground,odom vertical direction, and pose transformation. A window that is too narrow will lose the slope surface, while one that is too wide will introduce the ceiling or other floors; whether it becomes an obstacle also depends on local ground estimation and subsequent filtering. Currently, the gravity direction or installation extrinsics are not directly modified based on these values.

1.2.1.2.3.useSorting and quantileZ: why a normal slope can also look like an obstacle

The extended node partitions horizontal ground into hardcoded 0.4 m cells. Each collects nearby heights and selects a representative ground height for the region. Currently it sorts the heights and selects the value at floor(0.2×样本数) ; with 10 samples, this is the third-lowest. False instead selects the minimum height. When a cell and its neighborhood contain slopes, obstacles or outliers, this representative height may differ from the true ground beneath each point.

This node's own far field is always computed abs(点高度−估计地面高度), with no considerDrop switch. A point that is 15 cm above the estimated ground, or 15 cm below it, will both output a height difference of 0.15 m; when other conditions are met, either may reach the 10 cm obstacle threshold of the global costmap.

This is especially what to look at on a downhill slope: the lower side of a normal slope may simply be the representative ground lower than this small patch, and is not a true pit. Conversely, a true depression can be represented through this height difference only if it is seen by radar and leaves a valid echo; a cliff with no echo does not automatically become reliably identified because of this. The expansion node also does not have the near-field noDataObstacle synthetic obstacle function; how areas without points affect planning still depends on the downstream unknown-area settings.

Increasing quantileZ usually raises the represented ground: the height difference at higher points may decrease, while the height difference at lower points may increase. Therefore, one cannot simply assume that “increasing it makes going up and down slopes easier.” One should consider normal slopes, small steps, and low obstacles together to confirm whether the ground estimate is too high or too low. Here there is no cancellation by adding positive and negative height differences, nor is there a dedicated switch to allow uphill or prohibit downhill.

1.2.1.2.4.vehicleHeight: Which obstacle points can enter the far-field output?

Currently, only points with an absolute height difference of h<1.0 m are retained in the self far field, while the 1.0 m boundary is not retained. This value does not change the robot dimensions, nor does it mean that a 1 m step can be crossed.

For example: if an obstacle has a valid echo at 20 cm above the ground, it may enter the output; if it only has an echo at 1.2 m above the ground, and the "ground height" is selected correctly, these echoes will be removed by this item. Reducing it may miss some obstacles, while increasing it may introduce structures such as suspended beams into the result. It should be checked against the height that the real vehicle needs to avoid, the parts that the radar can see, and the ground estimate; the name must not be directly filled in as the vehicle height.

When acting together with the downstream 10 cm threshold, the range of elevation differences for its own far-field obstacle screening entry is 0.1 to less than 1.0 m, and other conditions are still required. Near-field points borrowed within 4 m continue to use the current near-field upper limit of 0.5 m and are not subject to this re-screening.

1.2.1.2.5.localTerrainMapRadius: why should the vicinity of 4 m be examined separately?

Currently centered on the odometry input position:d≤4 m Directly duplicate terrain_map points;d>4 m Use extended nodes to compute points themselves. Near-field points will not reapply this node's vehicleHeight sorting, height window, attenuation, or connection checks.

The current differences between the two sides are as follows:

Therefore, as the vehicle approaches the same slope or object, the height difference or visible point may change when crossing 4 m; this is a source-code difference that needs to be observed, and no actual handover failure has been confirmed so far. Expanding the radius will borrow more near-field results, while shrinking it will use coarser far-field results earlier; it does not change the costmap size, radar range, or vehicle radius.

Two further interface prerequisites:If the near field has no points, the extended node does not fill gaps with its own calculations inside 4 m; if near-field updates stop, the code does not check their age. It continues merging the latest received near-field result and timestamps the merged message with the current scan time. A fresh terrain_map_ext timestamp therefore does not establish that every point in the message is fresh.

1.2.1.2.6.checkTerrainConn and three accompanying parameters: currently, it is known that they have not taken effect.

Currently checkTerrainConn=False therefore terrainUnderVehicle、terrainConnThre、ceilingFilteringThre do not affect the output. Other altitude filtering is still being performed; here only the dedicated connection check and ceiling filtering are disabled.

If activation is considered in the future, it starts from the ground near the input position, tries to connect ground areas with similar elevations; its own far field outputs only the connected areas. The three meanings are:

• terrainUnderVehicle=-0.75 m：Only when there is no ground sample in the center cell, assume that the ground is 0.75 m below the input origin, as the starting point for checking. If the premise holds, the relationship from that origin to the ground should be measured; it is not the chassis ground clearance, nor a directly read actual vehicle height.
• terrainConnThre=0.5 m：The height difference between two candidate ground patches must be less than 0.5 m to connect them. If it is too small, a normal slope may be disconnected; if it is too large, a real step may be connected or a gap may be bridged; it does not mean that climbing a 50 cm step is allowed.
• ceilingFilteringThre=2.0 m：During connection checking, candidates whose height difference exceeds 2.0 m may be marked as excluded. The source code uses absolute difference, so regions far below the current ground may also match; it is not a ceiling line that deletes anything more than 2 m above the radar, nor is it roof clearance.

The source code can compare ±10 ground cells horizontally and vertically at a time, i.e., ±4 m each, not only checking the immediately adjacent small cells; whether there is continuous ground in between is not proven segment by segment. Therefore, this check also cannot be directly used as a criterion for vehicle traversability or reliable cliff detection. It is currently disabled, and there is no need to prioritize adjusting these three items; the applicability of enabling it must be verified separately.

1.2.1.2.7.scanVoxelSize, decayTime and update thresholds: fine obstacles and stale points

scanVoxelSize=0.1 mis the 3D downsampling scale of its own accumulated point cloud, not the ground grid size, and it does not guarantee that all obstacles above 10 cm can be seen. Thin poles, obstacle edges, and sparse echoes at long range should be observed with emphasis; reducing it can retain more details, but the number of points and the computational load will increase.

The historical cache uses hardcoded 2 m horizontal blocks. A block is downsampled again and checked for age only when it receives 100 new points, or 2.0 seconds have elapsed since its last update, or a clearing event occurs. New scans still drive these steps. decayTime=0.2 s is the expiration threshold used during the check and cannot override its trigger conditions.

Therefore, blocks with dense points nearby may be updated quickly, while blocks with few points far away may wait until the time threshold, with old points retained for more than 0.2 seconds. When there is no new scan, processing will not continue based solely on the timer.noDecayDis=0.0There is no decay-free region; if increased later, it only allows the self-history within a specified range to bypass the age check, and will not automatically restore the missing output in the near field.

These two update thresholds are not publish rates, and a 100 Hz loop in the source code does not mean outputting 100 frames per second. For now, keep using the thresholds; if in the physical setup you find leftover old obstacles, ground defects, or excessive burden, then judge together with point density and decay time. The extension node does not have that near-field group of clearDyObs dynamic filtering parameters, so it cannot be treated as the same cleanup algorithm.

1.2.1.2.8.clearingDis: clear within radius by event, and only clear one's own

The current 20 m means that after receiving a map-clearing event, cached points of the vehicle itself with a horizontal distance of less than 20 m are deleted. It is not automatically executed during normal operation, nor is it the maximum output range.

joy The 6th button can be triggered;cloud_clearing Float32 messages will also override the radius with the message value. The topic corresponding to the near-field node is map_clearing, and the two are different. After the extended node clears the map, it will still borrow terrain_map near-field results, so near-field points can continue to appear in the merged output; clearing its own cache cannot be understood as clearing the entire map. No map-clearing event was sent in this round.

1.2.1.2.9.Startup source and current verification scope

The entire navigation launch will simultaneously schedule two terrain analysis nodes. The extension node itself is always an independent process and does not become a component with the toggling of navigation components. In this round, within ysu_ros_humble only the environment and files are queried to confirm that the package path, the independent launch after installation, the navigation launch, and the two YAML files are consistent with the current source files; no nodes were started, and no valid parameters were read from running nodes.

All 17 items are declared and read at startup, with no dynamic parameter update callback; at runtime, merely writing parameters with the same names will not update the member variables in the algorithm.clearingDisIt can change the quantity via clear-map messages, which is a different matter from re-reading ROS parameters. The decay timing uses the input point cloud timestamp, not the computer wall clock; this block does not list use_sim_time, nor can it be used to prove that the full-chain clock or the intra-point time unit is already correct.

The input odometry and scan are not time-paired or interpolated, and there is no protection for waiting for valid odometry; the near-field point cloud likewise lacks synchronization and freshness checks. The node directly uses the input coordinates without validating the reference frame, yet at the end it fixedly writes odom. These premises affect both the slope height and the distance partitioning.

• Source code can confirm: The 17 reads and the current gating, the height window that expands with distance, the far-field absolute elevation difference, the 4 m near-far field merge, the attenuation and map-clearing trigger method, as well as the three inactive parameters; the behavioral explanation above has been reviewed against the current source code.
• Simulation has observed: The 2026-10-08 static and approximately 20 cm round-trip tests only covered the independent Point-LIO output, and did not validate extended terrain, uphill and downhill slopes, potholes, ceilings, the 4 m handover, or the global costmap.
• Physical verification is required: the height from the input origin to the ground, the internal IMU→LiDAR→chassis relationships,odom the vertical direction, the echo density of slopes and low obstacles, whether downhill and true elevation differences are correctly distinguished, consistency at the handover point, and residual traces after obstacles leave. The mechanical convention that MID360 rotates with actual yaw and not with actual pitch continues to be retained; no runtime tests were performed in this round.

Source code location: src/pb2025_sentry_nav/terrain_analysis_ext/src/terrainAnalysisExt.cpp; startup source: src/pb2025_sentry_nav/pb2025_nav_bringup/launch/navigation_launch.py and two copies of nav2_params.yaml; downstream height difference filtering: src/pb2025_sentry_nav/pb_nav2_plugins/src/layers/intensity_voxel_layer.cpp. This block only updates the log explanation; the engineering source code, configuration, and parameter values are all retained.