太好了！我们来用 C++ 和 OMPL 库 实现一个完整的 Informed RRT* vs RRT* 对比 Demo。

这个程序会：

• 在 2D 空间中规划路径
• 使用相同的环境（起点、终点、障碍物）
• 分别运行 RRT* 和 Informed RRT*
• 输出：是否找到解、最终路径长度、以及中间过程的优化趋势
• 可视化数据可通过输出文件绘图（如 Python / MATLAB）

---

✅ 第一步：安装 OMPL（C++ 版）

# Ubuntu/Debian
sudo apt-get install libompl-dev

# 或从源码编译（推荐，支持更多功能）：
git clone https://github.com/ompl/ompl.git
cd ompl && mkdir build && cd build
cmake ..
make -j4
sudo make install

---

✅ 第二步：完整 C++ 代码（compare_rrt_star.cpp）

// compare_rrt_star.cpp
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>

#include <iostream>
#include <fstream>
#include <vector>

namespace ob = ompl::base;
namespace og = ompl::geometric;

// 状态有效性检查：避开原点附近的圆形障碍
bool isStateValid(const ob::State *state)
{
    const auto *s = state->as<ob::RealVectorStateSpace::StateType>();
    double x = s->values[0];
    double y = s->values[1];

    // 圆形障碍物：半径 1.0
    return (x*x + y*y) >= 1.0;
}

// 运行指定的规划器，并记录优化过程
void planWithPlanner(const std::string& plannerName, 
                     const ob::PlannerPtr& planner,
                     og::SimpleSetup& ss,
                     std::ofstream& logFile)
{
    std::cout << "\n=== Running " << plannerName << " ===" << std::endl;

    // 设置优化目标：最小化路径长度
    auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(ss.getSpaceInformation()));
    ss.setOptimizationObjective(obj);

    ss.setPlanner(planner);

    // 设置起始和目标状态
    ob::ScopedState<> start(ss.getStateSpace());
    ob::ScopedState<> goal(ss.getStateSpace());
    start->as<ob::RealVectorStateSpace::StateType>()->values[0] = -4.0;
    start->as<ob::RealVectorStateSpace::StateType>()->values[1] = -4.0;
    goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 4.0;
    goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 4.0;

    ss.setStartAndGoalStates(start, goal);

    // 尝试求解（最多 10 秒或 10000 次采样）
    ob::PlannerStatus solved = ss.solve(10.0);
    
    if (solved)
    {
        std::cout << plannerName << ": Found solution!" << std::endl;

        // 简化路径
        ss.simplifySolution();

        const auto &path = ss.getSolutionPath();
        double length = path.length();
        double smoothness = path.smoothness();

        std::cout << plannerName << " Final Path Length: " << length << std::endl;

        // 记录到日志文件
        logFile << plannerName << "," << length << "," << path.getStateCount() << std::endl;

        // 可选：导出路径点用于绘图
        std::ofstream pf((plannerName + "_path.csv").c_str());
        path.printAsMatrix(pf);
        pf.close();
    }
    else
    {
        std::cout << plannerName << ": No solution found." << std::endl;
        logFile << plannerName << ",inf,0" << std::endl;
    }
}

int main()
{
    // 创建 2D 状态空间
    auto space(std::make_shared<ob::RealVectorStateSpace>(2));
    space->setBounds(-5.0, 5.0);  // x, y 范围

    // 创建简单设置
    og::SimpleSetup ss(space);
    ss.setStateValidityChecker(isStateValid);

    // 设置空间信息
    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.01);

    // 日志文件记录结果
    std::ofstream logFile("results.csv");
    logFile << "Planner,PathLength,NumStates\n";

    // === 1. RRT* ===
    auto rrtstar = std::make_shared<og::RRTstar>(ss.getSpaceInformation());
    rrtstar->setRange(0.5);           // 步长
    rrtstar->setGoalBias(0.05);       // 目标偏置
    rrtstar->setRewireFactor(1.2);    // 启用重布线
    planWithPlanner("RRT*", rrtstar, ss, logFile);

    // 清除上一次的结果
    ss.clear();

    // === 2. Informed RRT* ===
    auto informed_rrtstar = std::make_shared<og::InformedRRTstar>(ss.getSpaceInformation());
    informed_rrtstar->setRange(0.5);
    informed_rrtstar->setGoalBias(0.05);
    informed_rrtstar->setRewireFactor(1.2);
    planWithPlanner("InformedRRT*", informed_rrtstar, ss, logFile);

    logFile.close();

    std::cout << "\n✅ Results saved to 'results.csv' and path files." << std::endl;
    return 0;
}

---

✅ 第三步：编译与运行

创建 CMakeLists.txt：

# CMakeLists.txt
cmake_minimum_required(VERSION 3.10)
project(ompl_rrt_comparison)

set(CMAKE_CXX_STANDARD 17)

find_package(OMPL REQUIRED)

add_executable(compare_rrt_star compare_rrt_star.cpp)

target_link_libraries(compare_rrt_star ${OMPL_LIBRARIES})

然后编译并运行：

mkdir build && cd build
cmake ..
make

# 运行程序
./compare_rrt_star

---

✅ 输出说明

程序将生成：

• results.csv：包含两个算法的路径长度对比
• RRT*_path.csv 和 InformedRRT*_path.csv：路径坐标点（可用 Python 绘图）

示例内容（results.csv）：

Planner,PathLength,NumStates
RRT*,9.65,87
InformedRRT*,8.32,73

---

✅ 可视化路径（Python 脚本）

保存为 plot_paths.py：

import numpy as np
import matplotlib.pyplot as plt

def plot_path(filename, label, color):
    data = np.loadtxt(filename, delimiter=' ')
    plt.plot(data[:, 0], data[:, 1], '-o', label=label, color=color, markersize=4)

plt.figure(figsize=(8, 8))

plot_path('RRT*_path.csv', 'RRT*', 'red')
plot_path('InformedRRT*_path.csv', 'Informed RRT*', 'blue')

# 绘制障碍物
circle = plt.Circle((0, 0), 1.0, color='gray', alpha=0.5)
plt.gca().add_patch(circle)

# 设置范围和标题
plt.xlim(-5, 5)
plt.ylim(-5, 5)
plt.xlabel("X")
plt.ylabel("Y")
plt.title("RRT* vs Informed RRT* Path Comparison")
plt.legend()
plt.grid(True, alpha=0.3)
plt.axis('equal')
plt.show()

运行它：

python plot_paths.py

你会看到：

• Informed RRT* 更快绕过障碍，路径更接近直线
• RRT* 初始路径更曲折，收敛更慢

---

✅ 核心差异总结

特性	RRT*	Informed RRT*
采样区域	全局随机	椭圆区域（基于当前最优解）
收敛速度	较慢	✅ 更快
路径质量	渐进最优	✅ 更快逼近最优
实现复杂度	简单	中等（需维护启发式采样）
是否需要初始解	❌ 否	✅ 是（首次为全空间，之后缩小）

🔍 原理：Informed RRT* 使用 informed sampling —— 只在“有可能找到更好解”的椭球区域内采样，避免无效探索。

---

✅ 总结

你现在已经拥有了一个完整的 C++ Demo，可以：

• 编译运行，比较两种算法性能
• 导出数据进行分析或发表论文
• 扩展到高维空间（如机械臂）、加入动力学约束等

如果你想要：

• 加入 实时绘图
• 输出 每一步的代价变化曲线
• 支持 BIT*
• 或集成到 ROS / MoveIt

只要告诉我，我立刻帮你加上！🚀

#关于OMPL障碍物设置

非常好的问题！我们来一步步解答：

---

✅ 一、原代码中障碍物在哪里设置的？

在你提供的 C++ 代码中，障碍物是通过 状态有效性检查函数 isStateValid 定义的：

bool isStateValid(const ob::State *state)
{
    const auto *s = state->as<ob::RealVectorStateSpace::StateType>();
    double x = s->values[0];
    double y = s->values[1];

    // 圆形障碍物：半径 1.0，中心在 (0,0)
    return (x*x + y*y) >= 1.0;  // 如果点在圆外 → 有效；在圆内 → 无效
}

🔍 关键点：OMPL 不直接建模障碍物几何体，而是通过 isStateValid() 函数判断某个状态（如机器人位置）是否“碰撞”。
所以，所有障碍物逻辑都写在这个函数里。

---

✅ 二、如何改成 10 个矩形障碍物？

假设每个矩形由 (xmin, xmax, ymin, ymax) 定义。我们可以定义一个矩形列表，然后在 isStateValid 中逐个检测是否与任意矩形相交。

✅ 修改后的代码如下：

#include <vector>
#include <utility> // for std::pair

// 定义一个矩形：{xmin, xmax, ymin, ymax}
using Rectangle = std::array<double, 4>;

// 全局障碍物列表（你可以放在类里或命名空间中）
const std::vector<Rectangle> obstacles = {
    {-3.0, -2.5, -1.0,  1.0},   // 竖条
    { 2.0,  2.5, -2.0, -1.0},
    { 0.0,  1.0,  2.0,  3.0},
    {-1.5, -0.5,  3.0,  4.0},
    { 3.0,  4.0,  0.0,  1.0},
    {-4.0, -3.0, -4.0, -3.0},
    { 1.5,  2.5,  1.5,  2.5},
    {-2.0, -1.0, -3.0, -2.0},
    { 0.5,  1.5, -4.0, -3.0},
    {-0.5,  0.5, -0.5,  0.5}    // 中心小方块
};

bool isStateValid(const ob::State *state)
{
    const auto *s = state->as<ob::RealVectorStateSpace::StateType>();
    double x = s->values[0];
    double y = s->values[1];

    // 检查是否在任意矩形障碍物内部
    for (const auto& rect : obstacles) {
        double xmin = rect[0], xmax = rect[1];
        double ymin = rect[2], ymax = rect[3];

        if (x >= xmin && x <= xmax && y >= ymin && y <= ymax) {
            return false; // 碰撞！无效状态
        }
    }

    return true; // 未碰撞，有效
}

✅ 说明：

• 矩形是 轴对齐（AABB），即边平行于坐标轴。
• 如果你需要旋转矩形，需用更复杂的点-多边形包含判断（可用 Eigen 或自定义）。
• 这种方式适用于静态障碍物。动态障碍物需结合时间维度（OMPL 支持时态规划，但更复杂）。

---

✅ 三、完整整合建议

把 obstacles 定义放在 main() 外部（全局或命名空间），或者封装成一个类（推荐用于大型项目）。

如果你希望 可视化这些矩形，可以在 Python 绘图脚本中添加：

import matplotlib.patches as patches

# 在 plot_paths.py 的 plt.figure() 后添加：
for rect in [
    [-3.0, -2.5, -1.0,  1.0],
    [ 2.0,  2.5, -2.0, -1.0],
    # ... 其他9个
]:
    xmin, xmax, ymin, ymax = rect
    width = xmax - xmin
    height = ymax - ymin
    rect_patch = patches.Rectangle((xmin, ymin), width, height,
                                   linewidth=1, edgecolor='black', facecolor='gray', alpha=0.6)
    plt.gca().add_patch(rect_patch)

---

✅ 四、进阶提示（可选）

需求	建议
障碍物很多（>100）	用 空间加速结构（如 KD-Tree、Grid Hash）避免 O(n) 检测
任意多边形障碍	将障碍物表示为顶点列表，使用 射线法 判断点是否在多边形内
机器人有尺寸	使用 Minkowski 和膨胀障碍物，或将机器人视为点+膨胀障碍
动态障碍	引入时间维度，状态变为 (x, y, t)，障碍物随时间移动

---

✅ 总结

• 障碍物逻辑在 isStateValid() 函数中
• 10 个矩形 = 定义 10 个 [xmin,xmax,ymin,ymax]，循环检测点是否在其内部
• 返回 false 表示碰撞（无效），true 表示自由空间（有效）

你现在可以自由设计任意数量、任意位置的矩形障碍物了！如果需要我帮你生成随机矩形、避免重叠、或导出配置文件，也可以告诉我 😊