前言

• 我们上一期说道，如何使用octomap-server对传感器采集到的点云进行3D建图、保存与预览。

  • 【OctoMap ROS 3D建图】ROS Noetic 基于 octomap_server 实现三维地图构建、可视化与地图保存

• 本期我们来利用octomap-server发布的/octomap_full进行地图膨胀与带地面约束的A*规划。最终达到如下的效果：

• 膨胀后的地图与基于地面约束的A*算法

• 注：本文A*规划地面约束部分参考：https://github.com/6-robot/jie_3d_nav

---

0 前置

0-1 修复点云

• 由于本项目在mid360前方防止了一个d435用于其他任务，但是上一期我们使用mid360的点云进行建图的时候，会将d435扫描进建图数据，故因此我们必须对原始数据进行过滤
• 参考实现如下：

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

class PointCloudFilter
{
public:
    PointCloudFilter()
    {
        ros::NodeHandle nh;

        pub_ = nh.advertise<sensor_msgs::PointCloud2>("/livox/PointCloud_edit", 1);
        sub_ = nh.subscribe("/livox/PointCloud", 1, &PointCloudFilter::callback, this);

        nh.param("center_radius", center_radius_, 0.3);

        ROS_INFO("PointCloudFilter started, center_radius=%.3f", center_radius_);
    }

private:
    void callback(const sensor_msgs::PointCloud2ConstPtr &msg)
    {
        pcl::PointCloud<pcl::PointXYZ> input;
        pcl::fromROSMsg(*msg, input);

        pcl::PointCloud<pcl::PointXYZ> output;
        output.header = input.header;
        output.is_dense = true;

        for (const auto &pt : input.points)
        {
            // 过滤 NaN
            if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z))
                continue;

            
            float dist2 = pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;

            if (dist2 < center_radius_ * center_radius_)
                continue;

            output.points.push_back(pt);
        }

        sensor_msgs::PointCloud2 output_msg;
        pcl::toROSMsg(output, output_msg);

        output_msg.header = msg->header;
        pub_.publish(output_msg);
    }

private:
    ros::Subscriber sub_;
    ros::Publisher pub_;
    double center_radius_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "livox_center_filter_node");

    PointCloudFilter node;

    ros::spin();
    return 0;
}

• 同时我们必须确保发布点云的坐标系一定是真实的雷达坐标系，这是由于octomap_server建图的原理导致的，
  • OctoMap 建图本质是以“点云所属坐标系原点”为射线起点进行体素更新（raycasting）
• 因此发布给 octomap_server 的点云必须保持真实雷达坐标系（如 livox/mid360 frame），不能提前转换到 base_link，否则会导致射线起点错误，从而引起地图扭曲或错误占据
• 同时必须保证 TF 树正确且时间戳一致，否则 octomap 在进行坐标变换时会产生插值误差甚至更新失败。

---

0-2 octomap_server参数修复

• 同时为了防止远距离的点云散点对建图效果进行影响，推荐降低点云的最大有效距离sensor_model/max_range，同时我们需要打开filter_speckles进行散点过滤。
• 需要额外备注的几个参数
  • prob_hit：当激光命中某个体素时，增加其为“占据”的概率
  • prob_miss：当射线穿过某个体素未命中时，降低其“占据概率”
  • occupancy_thres：体素被判定为“真正障碍物”的阈值
• 总结就是：

hit 提升占据置信度，miss 降低占据置信度，最终超过 occupancy_thres 才算真正障碍物

<launch>

    <param name="/use_sim_time" value="true"/>

    <node pkg="octomap_server"
        type="octomap_server_node"
        name="octomap_server"
        output="screen">

        <remap from="cloud_in" to="/scan2"/>

        <!-- map frame -->
        <param name="frame_id" value="map"/>

        <!-- 分辨率 -->
        <param name="resolution" value="0.1"/>

        <!-- 关键：限制最大有效距离 -->
        <param name="sensor_model/max_range" value="2.0"/>

        <!-- Z过滤 -->
        <param name="pointcloud_min_z" value="-0.5"/>
        <param name="pointcloud_max_z" value="7.0"/>

        <!-- ground filter -->
        <param name="filter_ground" value="false"/>
        <param name="filter_speckles" value="true"/>

        <param name="ground_filter/distance" value="0.2"/>
        <param name="ground_filter/angle" value="0.3"/>

        <!-- 防稀疏污染关键 -->
        <param name="prob_hit" value="0.7"/>
        <param name="prob_miss" value="0.4"/>
        <param name="occupancy_thres" value="0.65"/>

    </node>

</launch>

• 同时我们编写下述脚本用于地图保存时候自动带上时间信息

#!/bin/bash

# 保存路径
SAVE_DIR="/root/SimEnv/src/slam/maps"
mkdir -p "$SAVE_DIR"

# 时间戳
TIME_STR=$(date +"%Y%m%d_%H%M%S")

# 文件路径
BT_FILE="${SAVE_DIR}/map_${TIME_STR}.bt"
OT_FILE="${SAVE_DIR}/map_${TIME_STR}.ot"

echo "[INFO] Saving OctoMap..."

echo "[INFO] Saving BT to: $BT_FILE"
rosrun octomap_server octomap_saver "$BT_FILE"

echo "[INFO] Saving OT to: $OT_FILE"
rosrun octomap_server octomap_saver "$OT_FILE"

echo "[INFO] Done."

---

0-3 本次测试环境说明

• 本次测试环境为一个三层楼的建筑，加上楼梯。为了确保观察路径方便，这里只建立了一半的地图用于可视化。
  

---

1 膨胀地图

1-1 膨胀地图的意义

• 在基于栅格地图或八叉树地图（octomap）的路径规划中，原始占据地图通常只表示“物体的真实边界”，但并不能直接用于安全导航。
• 这是因为：
  • 机器人本身具有体积（非质点）
  • 控制存在误差（定位误差、控制延迟）
  • 传感器存在噪声（点云抖动、漏检）
• 因此，如果直接在原始占据体素上进行A* / RRT 等规划，路径可能会：
  • 紧贴障碍物（风险极高）
  • 穿越“理论可行但实际碰撞”的狭窄通道
  • 对小噪声极度敏感
• 膨胀地图（Inflated Map）的核心目的就是：

将障碍物“变粗”，为机器人保留安全冗余空间（Safety Margin）

• 举个2D导航的例子：

• 在没有进行膨胀之前，A*规划的路径会直接从墙体的缝隙直接穿过去，但是膨胀后的地图就完美的解决这个问题。

---

1-2 核心思路

• 这里我们实现最基础的逻辑： 基于体素邻域扩展的离散膨胀方法，核心思想如下：
• 对于每个占据点 $p$，在其周围构建一个立方体邻域：$$p+(dx,dy,dz),dx,dy,dz\in [-r,r]$$

其中：

• $$r=\lceil \frac{inflation\_radius}{resolution}\rceil$$
• resolution 为 OctoMap 分辨率

void buildInflated()
{
	inflated_tree_ = std::make_shared<octomap::OcTree>(resolution_);
	inflated_points_.clear();

	double res = resolution_;
	int r = std::ceil(inflation_radius_ / res);

	for (const auto& p : occupied_)
	{
		for (int dx = -r; dx <= r; dx++)
		for (int dy = -r; dy <= r; dy++)
		{
			int dz_start = use_3d_inflation_ ? -r : 0;
			int dz_end   = use_3d_inflation_ ?  r : 0;

			for (int dz = dz_start; dz <= dz_end; dz++)
			{
				octomap::point3d q(
					p.x() + dx * res,
					p.y() + dy * res,
					p.z() + dz * res
				);

				
				inflated_tree_->updateNode(q, true);
				//可视化
				inflated_points_.push_back(q);
			}
		}
	}

	inflated_tree_->updateInnerOccupancy();
}

• 值得说明的是，本历程提供了一下几个参数：
  • resolution_：地图分辨率，需要与原始的.bt文件一致
  • inflation_radius_：膨胀半径
  • use_3d_inflation_：是否启用3D膨胀，对于四足或者其余需要落地的机器人，地面的膨胀是不需要的，但是对于uav无人机的规划，需要打开这个参数。

float resolution_ = 0.1;
float inflation_radius_ = 0.35;
bool use_3d_inflation_ = false;

---

1-3 完整实现

#include <ros/ros.h>

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>

#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>

#include <memory>
#include <vector>
#include <cmath>

class InflatedMapNode
{
public:
    InflatedMapNode()
    {
        ros::NodeHandle nh;

        sub_ = nh.subscribe("/octomap_full", 1, &InflatedMapNode::mapCallback, this);

        // 规划用（OctoMap）
        pub_octomap_ = nh.advertise<octomap_msgs::Octomap>("/inflated_octomap", 1, true);

        // 可视化用（Marker）
        pub_marker_ = nh.advertise<visualization_msgs::Marker>("/cost_heatmap", 1, true);

        inflation_radius_ = 0.2;
        use_3d_inflation_ = false;

        ROS_INFO("Inflated Map Node Started");
    }

private:

    
    void mapCallback(const octomap_msgs::Octomap::ConstPtr& msg)
    {
        std::unique_ptr<octomap::AbstractOcTree> base(
            octomap_msgs::fullMsgToMap(*msg)
        );

        tree_ = std::unique_ptr<octomap::OcTree>(
            dynamic_cast<octomap::OcTree*>(base.release())
        );

        if (!tree_)
        {
            ROS_ERROR("Octomap load failed");
            return;
        }

        resolution_ = tree_->getResolution();

        buildOccupied();
        buildInflated();
        publishOctomap();
        publishMarker();
    }

    //填充基础地图
    void buildOccupied()
    {
        occupied_.clear();

        for (auto it = tree_->begin_leafs(), end = tree_->end_leafs();
             it != end; ++it)
        {
            if (tree_->isNodeOccupied(*it))
                occupied_.push_back(it.getCoordinate());
        }

        ROS_INFO("Occupied: %zu", occupied_.size());
    }

    //膨胀
    void buildInflated()
    {
        inflated_tree_ = std::make_shared<octomap::OcTree>(resolution_);
        inflated_points_.clear();

        double res = resolution_;
        int r = std::ceil(inflation_radius_ / res);

        for (const auto& p : occupied_)
        {
            for (int dx = -r; dx <= r; dx++)
            for (int dy = -r; dy <= r; dy++)
            {
                int dz_start = use_3d_inflation_ ? -r : 0;
                int dz_end   = use_3d_inflation_ ?  r : 0;

                for (int dz = dz_start; dz <= dz_end; dz++)
                {
                    octomap::point3d q(
                        p.x() + dx * res,
                        p.y() + dy * res,
                        p.z() + dz * res
                    );

                    
                    inflated_tree_->updateNode(q, true);
                    //可视化
                    inflated_points_.push_back(q);
                }
            }
        }

        inflated_tree_->updateInnerOccupancy();
    }

    // 发布地图
    void publishOctomap()
    {
        octomap_msgs::Octomap msg;
        msg.header.frame_id = "map";
        msg.header.stamp = ros::Time::now();

        if (!octomap_msgs::fullMapToMsg(*inflated_tree_, msg))
        {
            ROS_ERROR("Octomap convert failed");
            return;
        }

        pub_octomap_.publish(msg);
    }

    // 发布可视化
    void publishMarker()
    {
        visualization_msgs::Marker marker;
        marker.header.frame_id = "map";
        marker.header.stamp = ros::Time::now();

        marker.ns = "inflated_heatmap";
        marker.id = 0;
        marker.type = visualization_msgs::Marker::CUBE_LIST;
        marker.action = visualization_msgs::Marker::ADD;

        marker.scale.x = resolution_;
        marker.scale.y = resolution_;
        marker.scale.z = resolution_;

        marker.color.r = 1.0;
        marker.color.g = 0.3;
        marker.color.b = 0.3;
        marker.color.a = 1;

        marker.points.reserve(inflated_points_.size());

        for (const auto& p : inflated_points_)
        {
            geometry_msgs::Point pt;
            pt.x = p.x();
            pt.y = p.y();
            pt.z = p.z();
            marker.points.push_back(pt);
        }

        pub_marker_.publish(marker);
    }

private:
    ros::Subscriber sub_;

    ros::Publisher pub_octomap_;
    ros::Publisher pub_marker_;

    std::unique_ptr<octomap::OcTree> tree_;
    std::shared_ptr<octomap::OcTree> inflated_tree_;

    std::vector<octomap::point3d> occupied_;
    std::vector<octomap::point3d> inflated_points_;

    float resolution_ = 0.1;
    float inflation_radius_ = 0.35;
    bool use_3d_inflation_ = false;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "inflated_map_node");

    InflatedMapNode node;

    ros::spin();
    return 0;
}

---

1-4 测试

• 我们直接打开rviz

• 这是膨胀前的地图

• 这是膨胀后的

• 可以看到，膨胀后的地图减少了很多缝隙，这对我们进行后续的A*规划有着很大的帮助。

---

1-5 扩展

• 注：本教程实现的基于体素邻域扩展的离散膨胀方法，仅将障碍物周围固定半径内的栅格直接标记为占据区域，从而实现安全空间的硬约束建模。该方法实现简单、计算效率高，但本质上属于二值化处理（occupied / free），无法表达距离信息的连续变化。

• 在更高级的路径规划与导航系统中，可以进一步引入代价地图（Costmap）机制，例如 costmap_2d 或 octomap 的距离场扩展，实现如下优化：
  • 距离障碍物越近，代价越高（连续衰减）
  • 允许规划算法在“高代价区域”中通过，但代价更大
  • 将“硬约束不可通行”转变为“软约束风险建模”
  • 可与 A* / Dijkstra / D* Lite 等算法直接耦合
• 相比离散膨胀方法，代价地图具有以下优势：
  • 表达能力更强（连续安全距离）
  • 路径更平滑（避免贴障碍物边界）
  • 更适合动态环境与多传感器融合
  • 可自然融合机器人尺寸、定位误差与不确定性模型
• 因此，离散膨胀方法可视为代价地图的一种简化特例，其核心思想是一致的：

通过引入安全边界约束，提高路径规划的鲁棒性与可执行性。

---

2 带地面约束的A*规划

2-1 A*

• A*（A-star）算法是一种经典的启发式图搜索算法，广泛应用于机器人路径规划与网格地图搜索中。其核心思想是在保证最优性的前提下，通过引入启发函数加速搜索过程。
• A*将环境建模为图结构，其中：
  • 节点（Node）表示可通行栅格或体素
  • 边（Edge）表示相邻节点之间的移动代价
  • 起点与终点分别对应机器人当前位置与目标位置

---

2-1-1 评价函数

• A*算法的核心评价函数为：$$f(n)=g(n)+h(n)$$
• 其中：
  • $g(n)$：从起点到当前节点的实际代价
  • $h(n)$：当前节点到目标的启发式估计代价，也就是我们述说的启发函数
  • $f(n)$：综合代价，用于指导搜索方向

2-1-2 启发函数

• 在二维或三维栅格环境中，常用启发函数包括：
  • 曼哈顿距离（Manhattan Distance）$$h(n)=|x_n-x_g|+|y_n-y_g|$$
  • 欧氏距离（Euclidean Distance）$$h(n)=\sqrt{(x_n-x_g{)}^2+(y_n-y_g{)}^2}$$

2-1-3 算法流程

1. 将起点加入Open List（待搜索集合）
2. 初始化$g(n)=0$，并计算 $f(n)=g(n)+h(n)$
3. 循环执行以下步骤：
   • 从 Open List 中取出 $f(n)$ 最小的节点作为当前节点
   • 将当前节点加入 Closed List（已访问集合）
   • 若当前节点为目标节点，则搜索结束
   • 否则扩展当前节点的邻居节点：
     • 若邻居在 Closed List 中，则跳过
     • 计算邻居的 $g(n)$、$h(n)$、$f(n)$
     • 若邻居不在 Open List 或找到更优路径，则更新其父节点与代价
4. 重复上述过程直到找到目标或 Open List为空

• 最终，通过回溯父节点指针即可得到从起点到终点的最优路径。

---

2-2 3D-A*领域

• 传统的2d-A*的领域分别八领域和四领域：

• 但是考虑带进入3D-A*，我们将考虑的是26领域

• 因此我们访问的时候只需要进行三次循环就可以了

  //邻居-3D 八连通扩展（26邻域）
    std::vector<Grid> getNeighbors(const Grid& g)
    {
        std::vector<Grid> nb;
        // x,y,z三种3x3x3=27-自己
        for(int dx=-1; dx<=1; dx++)
        for(int dy=-1; dy<=1; dy++)
        for(int dz=-1; dz<=1; dz++)
        {
            //减去自己
            if(dx==0 && dy==0 && dz==0) continue;
            nb.push_back({g.x+dx, g.y+dy, g.z+dz});
        }

        return nb;
    }

---

2-3 OctoMap查找

• 考虑到本文所使用的地图结构为八叉树形式的三维概率地图（octomap），而非传统的规则体素栅格（Voxel Grid），因此在A*路径规划过程中无法直接通过数组索引访问占据状态，需要引入“连续空间 → 离散网格 → 八叉树查询”的映射机制。

2-3-1 空间映射关系

• 在实现中，连续空间点 $p(x,y,z)$ 首先被离散化为体素索引：$$(x,y,z)\to (i,j,k)$$其中：

  • $i=\lfloor x/resolution\rfloor$
  • $j=\lfloor y/resolution\rfloor$
  • $k=\lfloor z/resolution\rfloor$

• 对应实现

Grid toGrid(const octomap::point3d& p)
{
    return {
        (int)std::floor(p.x()/resolution_),
        (int)std::floor(p.y()/resolution_),
        (int)std::floor(p.z()/resolution_)
    };
}

---

2-3-2 网格反向映射

• 为了在路径发布与可视化中恢复真实空间坐标，需要进行反向映射：$$(i,j,k)\to (x,y,z)$$
• 即将体素中心映射回连续空间：

octomap::point3d toWorld(const Grid& g)
{
    return {
        (float)((g.x+0.5)*resolution_),
        (float)((g.y+0.5)*resolution_),
        (float)((g.z+0.5)*resolution_)
    };
}

• 该步骤保证了路径点位于体素中心，从而提高路径几何一致性。

---

2-3-3 OctoMap占据查询机制

• 与传统Voxel Map直接索引不同，OctoMap采用八叉树结构存储空间占据信息，因此需要通过点查询方式进行状态判断：

bool isOccupied(const Grid& g)
{
    auto p = toWorld(g);
    auto node = tree_->search(p);
    return node && tree_->isNodeOccupied(node);
}

• 其核心逻辑为：
  • 将体素中心点转换为世界坐标
  • 在八叉树中进行节点搜索（search）
  • 判断节点是否存在且为 occupied 状态

---

2-4 哈希校验

• 在基于 A* 的三维路径规划实现中，节点通常以结构体（如 Grid）形式存在。由于搜索过程中需要频繁进行节点的插入、查询与状态更新，因此必须依赖高效的数据结构来保证计算性能。
• 本实现中通过自定义哈希函数，将三维网格坐标映射为唯一的哈希值，从而支持在 unordered_map 与 unordered_set 中进行快速查找。

struct Hash
{
    size_t operator()(const Grid& g) const
    {
        return ((std::hash<int>()(g.x) ^
                (std::hash<int>()(g.y)<<1))>>1) ^
                (std::hash<int>()(g.z)<<1);
    }
};

• 该方法通过位运算将 x、y、z 三个维度融合为一个 64-bit 哈希值

---

2-4-1 哈希校验的作用

• 哈希校验的核心作用是：

将三维离散网格坐标（x, y, z）转换为唯一键值，用于快速判断节点是否访问或是否存在。

• 在A*搜索过程中主要用于：
  • closed set：判断节点是否已经访问过
  • cost table：记录某节点当前最优代价 g(n)
  • parent table：记录路径回溯关系

2-4-2 为什么必须使用哈希

• 在三维栅格搜索中，如果不使用哈希结构，而采用普通数组或列表：
  • 查询复杂度：O(n)
  • 插入复杂度：O(1)
  • 总体搜索开销极高
• 而使用哈希结构后：
  • 平均查询复杂度：O(1)
  • 支持快速去重
  • 支持大规模三维空间搜索
• 因此在octomap对应的离散规划问题中，哈希结构是A*能够实时运行的关键优化之一。

---

2-5 地面约束

• 注：本文A*规划地面约束部分参考：https://github.com/6-robot/jie_3d_nav
• 在传统的 2D-A* 路径规划中，通常默认环境已经被压缩为二维可行平面（例如栅格地图中的 free/occupied），因此不需要显式判断“下方是否有支撑结构”。
• 但在三维体素环境（如octomap）中，仅判断当前体素是否为空并不足以保证路径的物理可执行性，尤其是对于需要站立支撑的机器人或者机器狗，这个约束十分重要
• 地面约束的核心判断逻辑为：

当前节点下方一定范围内必须存在占据体素（ground support）

• 这里参考https://github.com/6-robot/jie_3d_nav把地面约束分为：
  • 严格地面约束
  • 非严格地面约束

2-5-1 严格地面约束

• 严格模式仅检查当前节点正下方一个体素：

Grid below{g.x, g.y, g.z - 1};

• 判断其是否存在占据：
  • 若下方存在障碍物 → 认为有支撑
  • 否则 → 当前节点不可通行

---

2-5-2 非严格地面约束

• 非严格模式扩展了搜索范围，考虑一个局部体积区域：
  • XY方向：支持半径 support_xy_radius_cells
  • Z方向：支持深度 support_depth_cells
• 其核心思想为：

只要当前节点下方一定范围内存在任意支撑点，则认为该节点有效

for (int dz = 1; dz <= std::max(1, support_depth_cells); dz++)
{
    for (int dx = -support_xy_radius_cells; dx <= support_xy_radius_cells; dx++)
    {
        for (int dy = -support_xy_radius_cells; dy <= support_xy_radius_cells; dy++)
        {
            Grid below{g.x + dx,
                       g.y + dy,
                       g.z - dz};

            auto p = toWorld(below);
            auto node = tree_->search(p);

            if (node && tree_->isNodeOccupied(node))
            {
                return true;
            }
        }
    }
}

---

2-5-3 完整实现：

//脚下是否有路
//参考：https://github.com/6-robot/jie_3d_nav
bool hasGroundSupport(const Grid& g,
				  bool strict_direct_ground_support,
				  int support_xy_radius_cells,
				  int support_depth_cells)
{
	//严格模式
	if (strict_direct_ground_support)
	{
		Grid below{g.x, g.y, g.z - 1};

		auto p = toWorld(below);
		auto node = tree_->search(p);

		return node && tree_->isNodeOccupied(node);
	}

	//非严格模式
	for (int dz = 1; dz <= std::max(1, support_depth_cells); dz++)
	{
		for (int dx = -support_xy_radius_cells; dx <= support_xy_radius_cells; dx++)
		{
			for (int dy = -support_xy_radius_cells; dy <= support_xy_radius_cells; dy++)
			{
				Grid below{g.x + dx,
						g.y + dy,
						g.z - dz};

				auto p = toWorld(below);
				auto node = tree_->search(p);

				if (node && tree_->isNodeOccupied(node))
				{
					return true;
				}
			}
		}
	}

	return false;
}

---

2-6 完整代码

#include <ros/ros.h>

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap/OcTree.h>

#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <cmath>
#include <memory>
#include <algorithm>

class Planner
{
public:
    Planner()
    {
        planPub_ = nh_.advertise<nav_msgs::Path>("/planned_path", 1);

        mapSub_ = nh_.subscribe("/inflated_octomap", 1, &Planner::mapCallback, this);

        goalSub_ = nh_.subscribe("/clicked_point", 1,&Planner::goalCallback, this);

        start_[0] = 0.0;
        start_[1] = -1.0;
        start_[2] = 0.1;

        ROS_INFO("ROS1 A* Planner (ROS2 migrated version) started");
    }

private:

    //地图回调
    void mapCallback(const octomap_msgs::Octomap::ConstPtr& msg)
    {
        octomap::AbstractOcTree* base = octomap_msgs::fullMsgToMap(*msg);
        auto* tree = dynamic_cast<octomap::OcTree*>(base);

        if (!tree)
        {
            ROS_ERROR("Octomap convert failed");
            delete base;
            return;
        }

        tree_.reset(tree);
        resolution_ = tree_->getResolution();

        ROS_INFO("Octomap updated");
    }

    // 目标点
    void goalCallback(const geometry_msgs::PointStamped::ConstPtr& msg)
    {
        if (!tree_) return;

        goal_[0] = msg->point.x;
        goal_[1] = msg->point.y;
        goal_[2] = msg->point.z;

        ROS_INFO("Start: [%.2f %.2f %.2f]", start_[0], start_[1], start_[2]);
        ROS_INFO("Goal : [%.2f %.2f %.2f]", goal_[0], goal_[1], goal_[2]);

        auto path = planAstar();
        publishPath(path);
    }

    // 网格
    struct Grid
    {
        int x,y,z;

        bool operator==(const Grid& o) const
        {
            return x==o.x && y==o.y && z==o.z;
        }
    };
    //哈希
    struct Hash
    {
        size_t operator()(const Grid& g) const
        {
            return ((std::hash<int>()(g.x) ^
                    (std::hash<int>()(g.y)<<1))>>1) ^
                    (std::hash<int>()(g.z)<<1);
        }
    };
    //转为网格
    Grid toGrid(const octomap::point3d& p)
    {
        //把连续空间离散化
        return {
            (int)std::floor(p.x()/resolution_),
            (int)std::floor(p.y()/resolution_),
            (int)std::floor(p.z()/resolution_)
        };
    }
    //转回世界
    octomap::point3d toWorld(const Grid& g)
    {
        return {
            (float)((g.x+0.5)*resolution_),
            (float)((g.y+0.5)*resolution_),
            (float)((g.z+0.5)*resolution_)
        };
    }

    //是否被占据
    bool isOccupied(const Grid& g)
    {
        //用 voxel 坐标 转世界坐标查 octree
        auto p = toWorld(g);
        auto node = tree_->search(p);
        return node && tree_->isNodeOccupied(node);
    }

    //启发函数，欧式距离
    double heuristic(const Grid& a, const Grid& b)
    {
        double dx=a.x-b.x;
        double dy=a.y-b.y;
        double dz=a.z-b.z;
        return std::sqrt(dx*dx + dy*dy + dz*dz);
    }

    //邻居-3D 八连通扩展（26邻域）
    std::vector<Grid> getNeighbors(const Grid& g)
    {
        std::vector<Grid> nb;
        // x,y,z三种3x3x3=27-自己
        for(int dx=-1; dx<=1; dx++)
        for(int dy=-1; dy<=1; dy++)
        for(int dz=-1; dz<=1; dz++)
        {
            //减去自己
            if(dx==0 && dy==0 && dz==0) continue;
            nb.push_back({g.x+dx, g.y+dy, g.z+dz});
        }

        return nb;
    }
    //脚下是否有路
    //参考：https://github.com/6-robot/jie_3d_nav
    bool hasGroundSupport(const Grid& g,
                      bool strict_direct_ground_support,
                      int support_xy_radius_cells,
                      int support_depth_cells)
    {
        //严格模式
        if (strict_direct_ground_support)
        {
            Grid below{g.x, g.y, g.z - 1};

            auto p = toWorld(below);
            auto node = tree_->search(p);

            return node && tree_->isNodeOccupied(node);
        }

        //非严格模式
        for (int dz = 1; dz <= std::max(1, support_depth_cells); dz++)
        {
            for (int dx = -support_xy_radius_cells; dx <= support_xy_radius_cells; dx++)
            {
                for (int dy = -support_xy_radius_cells; dy <= support_xy_radius_cells; dy++)
                {
                    Grid below{g.x + dx,
                            g.y + dy,
                            g.z - dz};

                    auto p = toWorld(below);
                    auto node = tree_->search(p);

                    if (node && tree_->isNodeOccupied(node))
                    {
                        return true;
                    }
                }
            }
        }

        return false;
    }
    // A*
    std::vector<Grid> planAstar()
    {
        Grid start = toGrid({start_[0],start_[1],start_[2]});
        Grid goal  = toGrid({goal_[0],goal_[1],goal_[2]});

        //节点
        struct Node
        {
            Grid g;
            double g_cost;
            double f_cost;
        };
        //对比
        struct Cmp
        {
            bool operator()(const Node& a, const Node& b)
            {
                return a.f_cost > b.f_cost;
            }
        };

        std::priority_queue<Node,std::vector<Node>,Cmp> open;

        std::unordered_map<Grid,double,Hash> cost;
        std::unordered_map<Grid,Grid,Hash> parent;
        std::unordered_set<Grid,Hash> closed;

        open.push({start,0,heuristic(start,goal)});
        cost[start]=0;

        while(!open.empty()&&ros::ok())
        {
            Node cur=open.top(); open.pop();

            if(closed.count(cur.g)) continue;
            closed.insert(cur.g);

            if(cur.g == goal)
            {
                return reconstruct(parent, cur.g);
            }

            for(auto& nb : getNeighbors(cur.g))
            {
                //地面限制
                if (!hasGroundSupport(nb, true, 1, 2))continue;
                //障碍物
                if(isOccupied(nb)) continue;

                double ng = cur.g_cost + heuristic(cur.g, nb);

                if(!cost.count(nb) || ng < cost[nb])
                {
                    cost[nb]=ng;
                    parent[nb]=cur.g;

                    double f = ng + heuristic(nb, goal);
                    open.push({nb,ng,f});
                }
            }
        }

        ROS_WARN("No path found");
        return {};
    }
    
    // 回溯
    std::vector<Grid> reconstruct(
        std::unordered_map<Grid,Grid,Hash>& parent,
        Grid cur)
    {
        std::vector<Grid> path;
        path.push_back(cur);

        while(parent.count(cur))
        {
            cur = parent[cur];
            path.push_back(cur);
        }

        std::reverse(path.begin(), path.end());
        return path;
    }

    // 发布路径
    void publishPath(const std::vector<Grid>& path)
    {
        nav_msgs::Path msg;
        msg.header.frame_id = "map";
        msg.header.stamp = ros::Time::now();

        for(auto& g : path)
        {
            auto p = toWorld(g);

            geometry_msgs::PoseStamped ps;
            ps.header = msg.header;

            ps.pose.position.x = p.x();
            ps.pose.position.y = p.y();
            ps.pose.position.z = p.z();
            ps.pose.orientation.w = 1.0;

            msg.poses.push_back(ps);
        }

        planPub_.publish(msg);
        ROS_INFO("Path size: %zu", path.size());
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber mapSub_;
    ros::Subscriber goalSub_;
    ros::Publisher planPub_;

    std::unique_ptr<octomap::OcTree> tree_;

    double start_[3];
    double goal_[3];
    //分辨率
    double resolution_{0.1};
};

int main(int argc,char** argv)
{
    ros::init(argc,argv,"ros1_astar_planner");
    Planner p;
    ros::spin();
    return 0;
}

---

2-7 测试

• 这里我们3D-A*订阅的是膨胀后的地图，所以请确保第一节的内容已经配置完成且打开

• 3D-A*将订阅rviz发布的

• 并计算一条从起点到这个点的路径

• 
  
  

• 可以看到添加膨胀后的路径远离障碍物
  

---

小结

• 本文提出并实现了一种结合 OctoMap、26邻域扩展、地面约束与膨胀地图的三维 A* 路径规划方法，使机器人能够在考虑障碍物、安全冗余与可执行性约束的三维环境中生成稳定可靠的导航路径。

• 聪明的你一定发现了，上述规划的路径仍会经过一些危险区域，例如上楼梯的时候切内道

• 下一期我们将讲将如何利用rviz去直接编辑octomap生成不可通行的墙壁，并保存为.bt文件

• 如有错误，欢迎指出！

• 感谢观看！