【机器人轨迹优化基础】从原理到实现：支持动态膨胀的机器人A*局部路径规划开源算法【Github开源】【ROS2】【C++】

https://github.com/JackJu-HIT/A-star-ROS2/tree/master

// 初始化Ego Planner基础参数voidTrajectoryAndObstaclesPublisher::init_ego_planner_base(){    planner_ = std::make_shared();    Eigen::Vector2i map_size(map_x_size_,map_y_size_);    map_     = std::make_shared(map_resolution_,map_size);    map_->setInflateRadius(0.5);    Eigen::Vector2i map_size_2(500,500);    planner_->initGridMap(map_,map_size_2);  // 传入地图和尺寸}

/* * @Function: Ego Planner + RViz Interactive Input (Global Path + Obstacles) * @Create by: juchunyu@qq.com * @Date: 2026-1-11 20:00:01 */#include "trajectory_obstacles_publisher.h"#include #include "geometry_msgs/msg/quaternion.hpp"  // 确保包含四元数消息类型#include TrajectoryAndObstaclesPublisher::TrajectoryAndObstaclesPublisher()     : Node("ego_planner_interactive_node"),      has_valid_global_path_(false),      has_obstacles_(false),      should_plan_(false),      needs_replan_(false){    // 1. 创建发布者（可视化用）    global_path_pub_ = this->create_publisher<: class="code-snippet__variable">msg::Path>("visual_global_path", 10);    local_traj_pub_ = this->create_publisher<: class="code-snippet__variable">msg::Path>("visual_local_trajectory", 10);    obs_pub_ = this->create_publisher<: class="code-snippet__variable">msg::PointCloud2>("visual_obstacles", 10);    obs_local_pub_ = this->create_publisher<: class="code-snippet__variable">msg::PointCloud2>("visual_local_obstacles", 10);    // 2. 创建订阅者（接收RViz下发的数据）    // 全局路径输入方式1：直接发布Path消息    // rviz_global_path_sub_ = this->create_subscription<:msg::path>(    //     "/rviz_input_global_path",    //     10,    //     std::bind(&TrajectoryAndObstaclesPublisher::rviz_global_path_callback, this, std::placeholders::_1)    // );    goal_pose_sub_ = this->create_subscription<: class="code-snippet__variable">msg::PoseStamped>(        "/goal_pose",        10,        std::bind(&TrajectoryAndObstaclesPublisher::goal_pose_callback, this, std::placeholders::_1)    );    // // 障碍物输入方式1：发布PointCloud2消息    // rviz_obstacles_sub_ = this->create_subscription<:msg::pointcloud2>(    


    
//     "/rviz_input_obstacles",    //     10,    //     std::bind(&TrajectoryAndObstaclesPublisher::rviz_obstacles_callback, this, std::placeholders::_1)    // );    // 路径添加：使用Publish Point工具逐个添加    rviz_point_sub_ = this->create_subscription<: class="code-snippet__variable">msg::PointStamped>(        "/clicked_point",        10,        std::bind(&TrajectoryAndObstaclesPublisher::rviz_point_callback, this, std::placeholders::_1)    );    // 障碍物输入方式3：使用2D Pose Estimate工具添加    pose_estimate_sub_ = this->create_subscription<: class="code-snippet__variable">msg::PoseWithCovarianceStamped>(        "/initialpose",        10,        std::bind(&TrajectoryAndObstaclesPublisher::pose_estimate_callback, this, std::placeholders::_1)    );    // 3. 触发规划的话题    trigger_plan_sub_ = this->create_subscription<: class="code-snippet__variable">msg::Bool>(        "/trigger_plan",        10,        std::bind(&TrajectoryAndObstaclesPublisher::trigger_plan_callback, this, std::placeholders::_1)    );    // 4. 初始化Ego Planner基础配置    init_ego_planner_base();    // 5. 定时器：5Hz触发规划与发布    timer_ = this->create_wall_timer(        std::chrono::milliseconds(200),        std::bind(&TrajectoryAndObstaclesPublisher::publish_and_plan, this)    );    RCLCPP_INFO(this->get_logger(), "Interactive Ego Planner Node Ready!");    RCLCPP_INFO(this->get_logger(), "=== 使用方法 ===");    RCLCPP_INFO(this->get_logger(), "1. 添加全局路径：使用2D Nav Goal工具设置终点");    RCLCPP_INFO(this->get_logger(), "2. 添加障碍物（三种方法任选）：");    RCLCPP_INFO(this->get_logger(), "   - 方法A：使用Publish Point工具点击地图");    RCLCPP_INFO(this->get_logger(), "   - 方法B：使用2D Pose Estimate工具点击Grid任意位置（推荐）");    RCLCPP_INFO(this->get_logger(), "   - 方法C：发布PointCloud2到 /rviz_input_obstacles");    RCLCPP_INFO(this->get_logger(), "3. 触发规划：ros2 topic pub /trigger_plan std_msgs/Bool \"{data: true}\"");    RCLCPP_INFO(this->get_logger(), "4. 停止规划：ros2 topic pub /trigger_plan std_msgs/Bool \"{data: false}\"");    RCLCPP_INFO(this->get_logger(), "5. 支持多次规划：可以反复发送true/false控制规划");    RCLCPP_INFO(this->get_logger(), "");    RCLCPP_INFO(this->get_logger(), "=== RViz2设置步骤 ===");    RCLCPP_INFO(this->get_logger(), "1. 添加Grid显示");    RCLCPP_INFO(this->get_logger(), "2. 添加2D Nav Goal工具（话题：/goal_pose）");    RCLCPP_INFO(this->get_logger(), "3. 添加2D Pose Estimate工具（使用默认话题：/initialpose）");    RCLCPP_INFO(this->get_logger(), "4. 添加Publish Point工具（使用默认话题：/clicked_point）");    RCLCPP_INFO(this->get_logger(), "5. 添加PointCloud2显示（话题：/visual_obstacles）");    RCLCPP_INFO(this->get_logger(), "6. 添加Path显示（话题：/visual_global_path和/visual_local_trajectory）");}// 初始化Ego Planner基础参数void TrajectoryAndObstaclesPublisher::init_ego_planner_base(){    planner_ = std::make_shared();    Eigen::Vector2i map_size(map_x_size_,map_y_size_);    map_     = std::make_shared(map_resolution_,map_size);    map_->setInflateRadius(0.5);    Eigen::Vector2i map_size_2(500,500);    planner_->initGridMap(map_,map_size_2);  // 传入地图和尺寸}// 统一添加障碍物函数void TrajectoryAndObstaclesPublisher::


    
add_obstacle_at_position(double x, double y){    std::lock_guard<: class="code-snippet__variable">mutex> lock(data_mutex_);    ObstacleInfo obs;    obs.x = x;    obs.y = y;    obs.z = 0.0;
    obstacles_.push_back(obs);    has_obstacles_ = true;
    // 如果有障碍物更新且正在规划中，则标记需要重新规划    if (should_plan_) {        needs_replan_ = true;        RCLCPP_INFO(this->get_logger(), "障碍物更新，已标记需要重新规划");    }
    RCLCPP_INFO(this->get_logger(), "添加障碍物: (%.2f, %.2f), 总障碍物数量: %zu",                 x, y, obstacles_.size());}// 2D Pose Estimate回调函数void TrajectoryAndObstaclesPublisher::pose_estimate_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg){    std::lock_guard<: class="code-snippet__variable">mutex> lock(data_mutex_);   // 1. 更新机器人初始位姿（原有逻辑）    cur_pose_.x = msg->pose.pose.position.x;    cur_pose_.y = msg->pose.pose.position.y;    cur_pose_.z = 0;    // 计算偏航角（使用tf2或手动计算，确保正确）    tf2::Quaternion q(        msg->pose.pose.orientation.x,        msg->pose.pose.orientation.y,        msg->pose.pose.orientation.z,        msg->pose.pose.orientation.w);    cur_pose_.z = tf2::getYaw(q);    std::cout  << std::endl;    // 2. 触发规划逻辑（新增）    if (has_valid_global_path_) {  // 确保已有全局路径        needs_replan_ = true;  // 标记需要重新规划        if (should_plan_) {            RCLCPP_INFO(this->get_logger(), "初始位置更新，触发重新规划！");        } else {            RCLCPP_WARN(this->get_logger(), "初始位置已更新，但规划未启动！请先发送 /trigger_plan true 启动规划");        }    } else {        RCLCPP_WARN(this->get_logger(), "初始位置已更新，但无有效全局路径，无法规划！");    }}// 触发规划话题回调void TrajectoryAndObstaclesPublisher::trigger_plan_callback(const std_msgs::msg::Bool::SharedPtr msg){    // std::lock_guard<:mutex> lock(data_mutex_);    // if (msg->data && !flag_) {    //     if (has_valid_global_path_) {    //         should_plan_ = true;    //         needs_replan_ = true;  // 设置需要重新规划    //         RCLCPP_INFO(this->get_logger(), "规划已触发! 路径点: %zu, 障碍物: %zu",     //                    global_plan_traj_.size(), obstacles_.size());    //     } else {    //         RCLCPP_WARN(this->get_logger(), "无法触发规划: 没有有效的全局路径!");    //     }    //     flag_ = msg->data;    // } else if(!msg->data && flag_){    //     should_plan_ = false;    //     needs_replan_ = false;  // 停止时清除重新规划标志    //     obstacles_.clear();  // 清除障碍物    //     global_plan_traj_.clear();  // 清除路径    //     planned_traj.clear();  // 清除规划轨迹    //     obstacles_.clear();  // 清除障碍物    //     RCLCPP_INFO(this->get_logger(), "规划已停止.");    //     flag_ = msg->data;    // }}// 处理2D Nav Goal - 生成从起点到目标的直线路void TrajectoryAndObstaclesPublisher::goal_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg){    add_obstacle_at_position(msg->pose.position.x, msg->pose.position.y);}// 生成直线路径void TrajectoryAndObstaclesPublisher::generate_straight_path(const geometry_msgs::msg::PoseStamped& start, 


    
                                                           const geometry_msgs::msg::PoseStamped& goal){    global_plan_traj_.clear();    // 计算路径点数量（每0.1米一个点）    double dx = goal.pose.position.x - start.pose.position.x;    double dy = goal.pose.position.y - start.pose.position.y;    double distance = std::sqrt(dx*dx + dy*dy);    int num_points = std::max(2, static_cast<int>(distance / 0.1));    // 生成直线路径点    for (int i = 0; i < num_points; ++i) {        double ratio = static_cast<double>(i) / (num_points - 1);        PathPoint point;        point.x = start.pose.position.x + ratio * dx;        point.y = start.pose.position.y + ratio * dy;        point.z = 0.0;        global_plan_traj_.push_back(point);    }}// // 处理Publish Point点击 - 添加障碍物void TrajectoryAndObstaclesPublisher::rviz_point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg){    std::lock_guard<: class="code-snippet__variable">mutex> lock(data_mutex_);    // global_plan_traj_.clear();    PathPoint point;    point.x = msg->point.x;    point.y = msg->point.y;    point.z = 0; // 使用z作为theta角度    global_plan_traj_.push_back(point);    std::cout  << point.x endl;    has_valid_global_path_ = true;
    // 如果有路径更新且正在规划中，则标记需要重新规划    if (should_plan_)     {        needs_replan_ = true;        RCLCPP_INFO(this->get_logger(), "路径更新，已标记需要重新规划");    }}// 原有的全局路径回调void TrajectoryAndObstaclesPublisher::rviz_global_path_callback(const nav_msgs::msg::Path::SharedPtr msg){    if (msg->poses.empty())    {        RCLCPP_WARN(this->get_logger(), "从RViz接收到空的全局路径!");        has_valid_global_path_ = false;        return;    }    std::lock_guard<: class="code-snippet__variable">mutex> lock(data_mutex_);    global_plan_traj_.clear();    for (const auto& pose_stamped : msg->poses)    {        PathPoint path_point;        path_point.x = pose_stamped.pose.position.x;        path_point.y = pose_stamped.pose.position.y;        path_point.z = pose_stamped.pose.position.z;        global_plan_traj_.push_back(path_point);    }    has_valid_global_path_ = true;
    // 如果有路径更新且正在规划中，则标记需要重新规划    if (should_plan_) {        needs_replan_ = true;        RCLCPP_INFO(this->get_logger(), "路径更新，已标记需要重新规划");    }
    RCLCPP_INFO(this->get_logger(), "从RViz接收到全局路径 (大小: %zu)", global_plan_traj_.size());}// 原有的障碍物点云回调// void TrajectoryAndObstaclesPublisher::rviz_obstacles_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)// {//     if (msg->width == 0)//     {//         RCLCPP_WARN(this->get_logger(), "从RViz接收到空的障碍物!");//         has_obstacles_ = false;//         return;//     }//     std::lock_guard<:mutex> lock(data_mutex_);//     sensor_msgs::PointCloud2Iterator iter_x(*msg, "x");//     sensor_msgs::PointCloud2Iterator iter_y(*msg, "y");//     sensor_msgs::PointCloud2Iterator iter_z(*msg, "z");//     obstacles_.clear();//     for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)//     {//         ObstacleInfo obs;


    
//         obs.x = *iter_x;//         obs.y = *iter_y;//         obs.z = 0.0;//         obstacles_.push_back(obs);//     }//     has_obstacles_ = true;
//     // 如果有障碍物更新且正在规划中，则标记需要重新规划//     if (should_plan_) {//         needs_replan_ = true;//         RCLCPP_INFO(this->get_logger(), "障碍物更新，已标记需要重新规划");//     }
//     RCLCPP_INFO(this->get_logger(), "从RViz接收到障碍物 (数量: %zu)", obstacles_.size());// }// 核心逻辑：检查数据更新→触发规划→发布结果void TrajectoryAndObstaclesPublisher::publish_and_plan(){    std::lock_guard<: class="code-snippet__variable">mutex> lock(data_mutex_);    // 如果有全局路径且应该规划，并且需要重新规划，则进行规划    if (needs_replan_ )//&& has_valid_global_path_ && needs_replan_)    {        // 设置全局路径        std::vector global_plan_traj_temp;        if (!global_plan_traj_.empty())        {            discretize_trajectory(global_plan_traj_, global_plan_traj_temp, 0.1);
            float mindist = 100000000;            int minddex = 0;            for(int i = 0; i < global_plan_traj_temp.size();i++)            {                double dist =  distance(global_plan_traj_temp[i], cur_pose_);                 if(dist < mindist)                {                    mindist = dist;                    minddex = i;                } 
            }            std::vector global_plan_traj_after;            global_plan_traj_after.push_back(cur_pose_);            for(int i = minddex; i < global_plan_traj_temp.size();i++)            {                global_plan_traj_after.push_back(global_plan_traj_temp[i]);            }            discretize_trajectory(global_plan_traj_after, global_plan_traj_temp, 0.1);            // ego_planner_->setPathPoint(global_plan_traj_temp);        }        // std::cout << "cur_pose_x =" << cur_pose_.x << "cur_pose_y =" << cur_pose_.y << std::endl;        // 设置障碍物        // ego_planner_->setObstacles(obstacles_);        for(int i = 0; i < obstacles_.size();i++)        {              Eigen::Vector2d coord(obstacles_[i].x,obstacles_[i].y);            // std::cout << " "            Eigen::Vector2i res;            res = map_->worldToGrid(coord);            map_->setObstacle(res, true);        }        Eigen::Vector2d start_pt(cur_pose_.x, cur_pose_.y);  // 自由空间        Eigen::Vector2d end_pt(global_plan_traj_temp[global_plan_traj_temp.size()-1].x, global_plan_traj_temp[global_plan_traj_temp.size()-1].y);      // 自由空间       std::cout  << std::endl;        // 5. 执行A*搜索        // if(inflate_flag_)        {            map_->inflate();  //only inflate point once            inflate_flag_ = false;        }        auto start = std::chrono::system_clock::now();        bool success = planner_->AstarSearch(0.1, start_pt, end_pt);  // 步长=分辨率        auto end = std::chrono::system_clock::now();        auto elapsed = std::chrono::duration_cast<: class="code-snippet__variable">chrono::milliseconds>(end - start);        printf("a_star_Planner Total Running Time: %d  ms \n", elapsed.count());        std::cout  << start_pt endl


    
;
        if (success)         {            planned_traj.clear();            std::vector<Eigen::Vector2d> path = planner_->getPath();  // 注意：getPath需改为返回Vector2d            std::cout ;            for (const auto& pt : path)             {                PathPoint temp;                temp.x = pt.x() ;                temp.y = pt.y();                temp.z = 0;                planned_traj.push_back(temp);                std::cout  << pt.x() y()             }        }         else        {            std::cout ;        }
        if (has_obstacles_) {            RCLCPP_INFO(this->get_logger(), "规划完成! 包含障碍物避让. 路径点: %zu, 障碍物: %zu",                        global_plan_traj_.size(), obstacles_.size());        } else {            RCLCPP_INFO(this->get_logger(), "规划完成! 无障碍物. 路径点: %zu",                        global_plan_traj_.size());        }
        // 规划完成后，重置重新规划标志，但保持 should_plan_ 为 true        // 这样下次有数据更新时可以自动重新规划        // needs_replan_ = false;        needs_replan_ = false;    }    // 发布所有可视化数据（无论是否更新，保持实时显示）    publish_global_path();    publish_planned_trajectory();    publish_obstacles();    publish_local_obstacles();}// 发布可视化全局路径void TrajectoryAndObstaclesPublisher::publish_global_path(){    // if (global_plan_traj_.empty()) return;    nav_msgs::msg::Path visual_path;    visual_path.header.stamp = this->now();    visual_path.header.frame_id = "map";    for (const auto& path_point : global_plan_traj_)    {        geometry_msgs::msg::PoseStamped pose;        pose.header = visual_path.header;        pose.pose.position.x = path_point.x;        pose.pose.position.y = path_point.y;        pose.pose.position.z = path_point.z;        tf2::Quaternion q;        q.setRPY(0.0, 0.0, 0.0);        pose.pose.orientation.x = q.x();        pose.pose.orientation.y = q.y();        pose.pose.orientation.z = q.z();        pose.pose.orientation.w = q.w();        visual_path.poses.push_back(pose);    }    global_path_pub_->publish(visual_path);}// 发布Ego Planner规划后的局部轨迹void TrajectoryAndObstaclesPublisher::publish_planned_trajectory(){    // if (planned_traj.empty()) return;    nav_msgs::msg::Path visual_traj;    visual_traj.header.stamp = this->now();    visual_traj.header.frame_id = "map";    for (size_t i = 0; i < planned_traj.size(); ++i)    {        // std::cout << "[publish_planned_trajectory] x = " << planned_traj[i].x << " y =" << planned_traj[i].y << std::endl;        geometry_msgs::msg::PoseStamped pose;        pose.header = visual_traj.header;        pose.pose.position.x = planned_traj[i].x;        pose.pose.position.y = planned_traj[i].y;        pose.pose.position.z = planned_traj[i].z;        tf2::Quaternion q;        q.setRPY(0.0, 0.0, 0.0);        pose.pose.orientation.x = q.x();        pose.pose.orientation.y = q.y();        pose.pose.orientation.z = q.z();        pose.pose.orientation.w = q.w();        visual_traj.poses.push_back(pose);    }    local_traj_pub_->publish(visual_traj);}


    
// 发布可视化障碍物void TrajectoryAndObstaclesPublisher::publish_obstacles(){    // if (obstacles_.empty()) return;    sensor_msgs::msg::PointCloud2 visual_obs;    visual_obs.header.stamp = this->now();    visual_obs.header.frame_id = "map";    visual_obs.width = obstacles_.size();    visual_obs.height = 1;    visual_obs.is_dense = true;    sensor_msgs::PointCloud2Modifier modifier(visual_obs);    modifier.setPointCloud2FieldsByString(1, "xyz");    sensor_msgs::PointCloud2Iterator<float> iter_x(visual_obs, "x");    sensor_msgs::PointCloud2Iterator<float> iter_y(visual_obs, "y");    sensor_msgs::PointCloud2Iterator<float> iter_z(visual_obs, "z");    for (const auto& obs : obstacles_)    {        *iter_x = static_cast<float>(obs.x);        *iter_y = static_cast<float>(obs.y);        *iter_z = 0.0f;        ++iter_x;        ++iter_y;        ++iter_z;    }    obs_pub_->publish(visual_obs);}// 计算两点之间的欧氏距离（单位：米）double TrajectoryAndObstaclesPublisher::distance(const PathPoint& p1, const PathPoint& p2) {    double dx = p2.x - p1.x;    double dy = p2.y - p1.y;    return std::sqrt(dx*dx + dy*dy);}/** * 将轨迹离散为均匀间隔的点（间隔10cm） * @param original_trajectory 原始轨迹（由多个顶点组成的折线） * @param discrete_trajectory 输出的离散轨迹 * @param interval 间隔距离（单位：米，默认0.1米即10cm） */void TrajectoryAndObstaclesPublisher::discretize_trajectory(const std::vector& original_trajectory,                                                            std::vector& discrete_trajectory,                                                            double interval) {    if (original_trajectory.size() 2) {        std::cerr  << std::endl;        return;    }    discrete_trajectory.clear();    // 添加轨迹起点    // discrete_trajectory.push_back(cur_pose_);    discrete_trajectory.push_back(original_trajectory[0]);    // 遍历原始轨迹的每一段线段    for (size_t i = 0; i < original_trajectory.size() - 1; ++i) {        const PathPoint& start = original_trajectory[i];        const PathPoint& end = original_trajectory[i+1];        double seg_length = distance(start, end);  // 线段总长度        if (seg_length 1e-6) {  // 跳过长度接近0的线段（避免除零）            continue;        }        // 计算当前线段需要插入的点数（不含起点，含终点）        int num_points = static_cast<int>(seg_length / interval);        // 最后一个点到终点的距离（避免累积误差）        double last_interval = seg_length - num_points * interval;        // 生成线段上的离散点        for (int j = 1; j <= num_points; ++j) {            double ratio;            if (j < num_points) {                // 前num_points-1个点：按均匀间隔计算                ratio = (j * interval) / seg_length;            } else {                // 最后一个点：直接对齐到线段终点（避免累积误差）                ratio = 1.0;            }            // 线性插值计算点坐标            PathPoint p;            p.x = start.x + ratio * (end.x - start.x);            p.y = start.y + ratio * (end.y - start.y);            discrete_trajectory.push_back(p);        }


    
    }}void TrajectoryAndObstaclesPublisher::publish_local_obstacles(){    std::vector obstacles;
    std::vector<Eigen::Vector2d> inflated_cloud = map_->getObstaclePointCloud();    for (const auto& point : inflated_cloud)    {        ObstacleInfo temp;        temp.x = static_cast<float>(point.x()); // 解析 x        temp.y = static_cast<float>(point.y()); // 解析 y        obstacles.push_back(temp);    }    if (obstacles.empty()) return;    sensor_msgs::msg::PointCloud2 visual_obs;    visual_obs.header.stamp = this->now();    visual_obs.header.frame_id = "map";    // 正确设置点云大小    visual_obs.height = 1;    visual_obs.width = obstacles.size();    visual_obs.is_dense = true;    // 为点云分配字段    sensor_msgs::PointCloud2Modifier modifier(visual_obs);    modifier.setPointCloud2FieldsByString(1, "xyz");    modifier.resize(obstacles.size());   // 关键！分配空间    // 创建迭代器    sensor_msgs::PointCloud2Iterator<float> iter_x(visual_obs, "x");    sensor_msgs::PointCloud2Iterator<float> iter_y(visual_obs, "y");    sensor_msgs::PointCloud2Iterator<float> iter_z(visual_obs, "z");    for (const auto& obs : obstacles)    {        *iter_x = static_cast<float>(obs.x);        *iter_y = static_cast<float>(obs.y);        *iter_z = 0.0f;        ++iter_x;        ++iter_y;        ++iter_z;    }    obs_local_pub_->publish(visual_obs);}// 主函数：启动节点int main(int argc, char *argv[]){    rclcpp::init(argc, argv);    auto node = std::make_shared();    rclcpp::spin(node);    rclcpp::shutdown();    return 0;}

// 初始化Ego Planner基础参数void TrajectoryAndObstaclesPublisher::init_ego_planner_base(){    planner_ = std::make_shared();    Eigen::Vector2i map_size(map_x_size_,map_y_size_);    map_     = std::make_shared(map_resolution_,map_size);    map_->setInflateRadius(0.5);    Eigen::Vector2i map_size_2(500,500);    planner_->initGridMap(map_,map_size_2);  // 传入地图和尺寸}

#include "path_searching/dyn_a_star.h"using namespace std;using namespace Eigen;// 析构函数：释放二维网格节点内存AStar::~AStar(){    if (GridNodeMap_ == nullptr) return;    // 释放每个节点对象    for (int i = 0; i POOL_SIZE_(0); ++i)    {        if (GridNodeMap_[i] == nullptr) continue;        for (int j = 0; j POOL_SIZE_(1); ++j)        {            delete GridNodeMap_[i][j];  // 释放节点        }        delete[] GridNodeMap_[i];  // 释放y方向指针数组    }    delete[] GridNodeMap_;  // 释放x方向指针数组    GridNodeMap_ = nullptr;}// 初始化二维网格地图void AStar::initGridMap(shared_ptr occ_map, const Eigen::Vector2i pool_size){    POOL_SIZE_ = pool_size;    CENTER_IDX_ = pool_size / 2;  // 二维中心索引（x,y方向分别取半）    // 分配二维网格节点指针数组：[x][y]    GridNodeMap_ = new GridNodePtr*[POOL_SIZE_(0)];    for (int i = 0; i POOL_SIZE_(0); ++i)    {        GridNodeMap_[i] = new GridNodePtr[POOL_SIZE_(1)];        for (int j = 0; j POOL_SIZE_(1); ++j)        {            GridNodeMap_[i][j] = new GridNode;  // 初始化每个二维节点        }    }    grid_map_ = occ_map;}// 二维对角线启发函数（已适配）double AStar::getDiagHeu(GridNodePtr node1, GridNodePtr node2){    double dx = abs(node1->index(0) - node2->index(0));  // x方向索引差    double dy = abs(node1->index(1) - node2->index(1));  // y方向索引差    int diag = min(dx, dy);               // 对角线移动步数    double remaining = abs(dx - dy);      // 剩余轴方向移动步数    return diag * sqrt(2.0) + remaining * 1.0;  // 对角线成本+轴方向成本}// 二维曼哈顿启发函数（移除z轴）double AStar::getManhHeu(GridNodePtr node1, GridNodePtr node2){    double dx = abs(node1->index(0) - node2->index(0));  // x方向差    double dy = abs(node1->index(1


    
) - node2->index(1));  // y方向差    return dx + dy;  // 仅x+y方向求和}// 二维欧式距离启发函数（移除z轴影响）double AStar::getEuclHeu(GridNodePtr node1, GridNodePtr node2){    // 仅计算x、y方向的欧式距离    return (node2->index - node1->index).head(2).norm();}// 回溯路径（无需修改，与维度无关）vector AStar::retrievePath(GridNodePtr current){    vector path;    path.push_back(current);    while (current->cameFrom != nullptr)    {        current = current->cameFrom;        path.push_back(current);    }    return path;}// 转换坐标到索引并调整起点终点（适配二维）bool AStar::ConvertToIndexAndAdjustStartEndPoints(Vector2d start_pt, Vector2d end_pt,                                                   Vector2i &start_idx, Vector2i &end_idx){    // 仅使用x、y坐标转换为二维索引（忽略z轴）    if (!Coord2Index(start_pt, start_idx) || !Coord2Index(end_pt, end_idx))        return false;    // // 检查起点是否在障碍物内，若在则向终点方向调整（仅x、y方向）    // if (checkOccupancy(Index2Coord(start_idx)))    // {    //     do    //     {    //         // 沿起点到终点方向移动一步（仅x、y）    //         Vector2d dir = (end_pt - start_pt).normalized();    //         start_pt += dir * step_size_;    //         if (!Coord2Index(start_pt, start_idx))    //             return false;    //     } while (checkOccupancy(Index2Coord(start_idx)));    // }    // // 检查终点是否在障碍物内，若在则向起点方向调整（仅x、y方向）    // if (checkOccupancy(Index2Coord(end_idx)))    // {    //     do    //     {    //         // 沿终点到起点方向移动一步（仅x、y）    //         Vector2d dir = (start_pt - end_pt).normalized();    //         end_pt += dir * step_size_;    //         if (!Coord2Index(end_pt, end_idx))    //             return false;    //     } while (checkOccupancy(Index2Coord(end_idx)));    // }    std::cout  << start_pt     return true;}// 二维A*搜索主函数bool AStar::AstarSearch(const double step_size, Vector2d start_pt, Vector2d end_pt){    ++rounds_;  // 搜索轮次计数（用于节点状态重置）    step_size_ = step_size;    inv_step_size_ = 1.0 / step_size;    center_ = (start_pt + end_pt) / 2.0;  // 中心位置（保留z但不影响二维搜索）    Vector2i start_idx, end_idx;  // 二维索引    if (!ConvertToIndexAndAdjustStartEndPoints(start_pt, end_pt, start_idx, end_idx))    {        printf("无法处理起点或终点，强制返回！\n");        return false;    }    // 获取起点和终点的节点指针（二维索引）    GridNodePtr startPtr = GridNodeMap_[start_idx(0)][start_idx(1)];    GridNodePtr endPtr = GridNodeMap_[end_idx(0)][end_idx(1)];    // 清空开放集    std::priority_queue, NodeComparator> empty;    openSet_.swap(empty);    GridNodePtr neighborPtr = nullptr;    GridNodePtr current = nullptr;    // 初始化起点节点    startPtr->index = start_idx;


    
    startPtr->rounds = rounds_;    startPtr->gScore = 0.0;    startPtr->fScore = getManhHeu(startPtr, endPtr);//getHeu(startPtr, endPtr);  // 启发函数（根据配置选择）    startPtr->state = GridNode::OPENSET;    startPtr->cameFrom = nullptr;    openSet_.push(startPtr);    endPtr->index = end_idx;  // 初始化终点索引    double tentative_gScore;  // 临时g值（代价）    int num_iter = 0;         // 迭代计数    while (!openSet_.empty())    {        ++num_iter;        current = openSet_.top();  // 取出f值最小的节点        openSet_.pop();        // 检查是否到达终点（仅x、y维度）        if (current->index(0) == endPtr->index(0) && current->index(1) == endPtr->index(1))        {            gridPath_ = retrievePath(current);  // 回溯路径            return true;        }        current->state = GridNode::CLOSEDSET;  // 移至关闭集        // 生成二维邻居节点（x和y方向偏移：-1,0,1，排除自身）        for (int dx = -1; dx <= 1; ++dx)        {            for (int dy = -1; dy <= 1; ++dy)            {                if (dx == 0 && dy == 0)  // 跳过当前节点                    continue;                // 计算邻居节点索引（二维）                Vector2i neighborIdx;                neighborIdx(0) = current->index(0) + dx;                neighborIdx(1) = current->index(1) + dy;                // 检查邻居是否在网格边界内（仅x、y维度）                if (neighborIdx(0) 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 ||                    neighborIdx(1) 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1)                {                    continue;                }                // 获取邻居节点指针                neighborPtr = GridNodeMap_[neighborIdx(0)][neighborIdx(1)];                neighborPtr->index = neighborIdx;                // 检查是否已在当前轮次处理过                bool flag_explored = (neighborPtr->rounds == rounds_);                // 若在关闭集则跳过                if (flag_explored && neighborPtr->state == GridNode::CLOSEDSET)                    continue;                // 标记当前轮次                neighborPtr->rounds = rounds_;                // 检查邻居是否为障碍物                if (checkOccupancy(Index2Coord(neighborPtr->index)))                    continue;                // 计算移动代价（二维欧氏距离：dx和dy的平方和开方）                double static_cost = sqrt(dx*dx + dy*dy);                tentative_gScore = current->gScore + static_cost;                if (!flag_explored)  // 新节点：加入开放集                {                    neighborPtr->state = GridNode::OPENSET;                    neighborPtr->cameFrom = current;                    neighborPtr->gScore = tentative_gScore;                    neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);                    openSet_.push(neighborPtr);                }                else if (tentative_gScore < neighborPtr->gScore)  // 已在开放集：更新代价                {                    neighborPtr->cameFrom = current;                    neighborPtr->gScore = tentative_gScore;                    neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);                }            }        }    }    // 开放集为空且未找到终点    return false;}// 获取最终路径（转换为坐标）


    
vector AStar::getPath(){    vector path;    for (auto ptr : gridPath_)    {        Vector2d coord = Index2Coord(ptr->index);  // 二维索引转坐标（z可设为0或保留原值）        path.push_back(coord);    }    reverse(path.begin(), path.end());  // 反转路径（从起点到终点）    return path;}

#include "grid_map.h"#include #include #include #include // 构造函数：初始化地图（世界尺寸→栅格数量，初始化原始/膨胀网格）GridMap2D::GridMap2D(double resolution, const Eigen::Vector2i& world_size)    : resolution_(resolution), inflate_radius_(0.5) {  // 默认膨胀半径0.5米    // 1. 校验输入合法性    if (resolution <= 0) {        throw std::invalid_argument("地图分辨率必须为正数（当前：" + std::to_string(resolution) + "）");    }    if (world_size.x() <= 0 || world_size.y() <= 0) {        throw


    
 std::invalid_argument("世界尺寸（x,y）必须为正数（当前：(" +             std::to_string(world_size.x()) + "," + std::to_string(world_size.y()) + ")）");    }    // 2. 计算栅格数量（世界尺寸 / 分辨率，向上取整确保覆盖完整世界范围）    int grid_cols = static_cast<int>(std::ceil(static_cast<double>(world_size.x()) / resolution));  // x方向→列数    int grid_rows = static_cast<int>(std::ceil(static_cast<double>(world_size.y()) / resolution));  // y方向→行数    map_size_.x() = grid_cols;    map_size_.y() = grid_rows;    // 3. 设置地图原点（世界坐标）：让地图中心对齐世界坐标原点    origin_.x() = -static_cast<double>(world_size.x()) / 2.0;  // 世界x范围：[origin.x(), origin.x()+world_size.x()]    origin_.y() = -static_cast<double>(world_size.y()) / 2.0;  // 世界y范围：[origin.y(), origin.y()+world_size.y()]    // 4. 初始化网格（原始障碍物网格 + 膨胀后网格）：默认全为自由空间（false）    original_grid_.resize(grid_rows, std::vector<bool>(grid_cols, false));  // 存储原始障碍物    grid_.resize(grid_rows, std::vector<bool>(grid_cols, false));          // 存储原始+膨胀障碍物    // 打印初始化信息    std::cout  << std::endl;    std::cout  << world_size.x() y()     std::cout  << resolution     std::cout  << grid_cols     std::cout  << std::fixed << std::setprecision(2) << origin_.x()               << origin_.x() + world_size.x()  << origin_.y()               << origin_.y() + world_size.y()  << std::endl;    std::cout  << inflate_radius_ }// 设置原始障碍物（同时更新原始网格和初始膨胀网格）void GridMap2D::setObstacle(const Eigen::Vector2i& grid_index, bool is_obstacle) {    // 1. 校验栅格索引合法性    if (!isIndexValid(grid_index)) {        std::cerr  << grid_index.x() y()                    << map_size_.x()-1 y()-1         return;    }    // 2. 更新原始障碍物网格（永久存储原始障碍物）    original_grid_[grid_index.y()][grid_index.x()] = is_obstacle;    // 3. 更新膨胀后网格（初始状态 = 原始网格，膨胀后会叠加膨胀区域）    grid_[grid_index.y()][grid_index.x()] = is_obstacle;    std::cout  << grid_index.x() y()                << (is_obstacle ? "障碍物" : "自由空间") << std::endl;}// 原始接口：检查点是否为【原始障碍物】（不包含膨胀区域）bool GridMap2D::isObstacle(const Eigen::Vector2d& world_pos) const {    // 1. 世界坐标→栅格索引    Eigen::Vector2i grid_index = worldToGrid(world_pos);    // 2. 超出地图范围视为障碍物（避免越界）    if (!isIndexValid(grid_index)) {        // std::cout << "[isObstacle] 点（" << world_pos.x() << "," << world_pos.y() << "）超出地图范围，视为原始障碍物" << std::endl;        return true;    }    // 3. 读取原始网格状态    bool is_obs = original_grid_[grid_index.y()][grid_index.x()];    // std::cout << "[isObstacle] 点（" << world_pos.x() << "," << world_pos.y() << "）→ 栅格（"     //           << grid_index.x() << "," << grid_index.y() << "），原始障碍物状态：" << is_obs << std::endl;    return is_obs;


    
}// 新增接口：检查点是否处于【膨胀后的障碍物区域】（原始障碍物+膨胀区）bool GridMap2D::getInflateOccupancy(const Eigen::Vector2d& world_pos) const {    // 1. 世界坐标→栅格索引    Eigen::Vector2i grid_index = worldToGrid(world_pos);    // 2. 超出地图范围视为占用（膨胀区延伸到地图边界外，避免路径超出地图）    if (!isIndexValid(grid_index)) {        // std::cout << "[getInflateOccupancy] 点（" << world_pos.x() << "," << world_pos.y() << "）超出地图范围，视为膨胀占用" << std::endl;        return true;    }    // 3. 读取膨胀后网格状态（原始+膨胀障碍物）    bool is_occupied = grid_[grid_index.y()][grid_index.x()];    // std::cout << "[getInflateOccupancy] 点（" << world_pos.x() << "," << world_pos.y() << "）→ 栅格（"     //           << grid_index.x() << "," << grid_index.y() << "），膨胀占用状态：" << is_occupied << std::endl;    return is_occupied;}// 带参膨胀接口：按指定半径膨胀障碍物（核心膨胀逻辑）void GridMap2D::inflateObstacles(double radius) {    // 1. 校验膨胀半径    if (radius <= 0) {        std::cerr  << radius         return;    }    std::cout  << radius     // 2. 计算膨胀半径对应的栅格数（向上取整，确保覆盖完整半径）    int inflate_grid_num = static_cast<int>(std::ceil(radius / resolution_));    std::cout  << inflate_grid_num     // 3. 收集所有原始障碍物的栅格索引（避免重复膨胀）    std::vector<:vector2i> original_obstacle_indices;    for (int row = 0; row < map_size_.y(); ++row) {  // 遍历所有行（y方向）        for (int col = 0; col < map_size_.x(); ++col) {  // 遍历所有列（x方向）            if (original_grid_[row][col]) {  // 若为原始障碍物                original_obstacle_indices.emplace_back(col, row);  // 存储（列，行）索引            }        }    }    if (original_obstacle_indices.empty()) {        std::cout  << std::endl;        return;    }    std::cout  << original_obstacle_indices.size()     // 4. 重置膨胀后网格（先恢复为原始障碍物状态，再叠加膨胀区域）    grid_ = original_grid_;    // 5. 对每个原始障碍物，膨胀周围栅格    int inflated_count = 0;  // 统计膨胀的栅格数    for (const auto& obs_idx : original_obstacle_indices) {        int obs_col = obs_idx.x();  // 障碍物列索引        int obs_row = obs_idx.y();  // 障碍物行索引        // 遍历以障碍物为中心，inflate_grid_num为半长的矩形范围（减少无效计算）        for (int dr = -inflate_grid_num; dr <= inflate_grid_num; ++dr) {  // 行方向偏移            for (int dc = -inflate_grid_num; dc <= inflate_grid_num; ++dc) {  // 列方向偏移                // 计算当前待检查的栅格索引                int curr_row = obs_row + dr;                int curr_col = obs_col + dc;                Eigen::Vector2i curr_idx(curr_col, curr_row);                // 跳过超出地图范围的栅格                if (!isIndexValid(curr_idx)) {                    continue;                }                // 若当前栅格已是原始障碍物，跳过（无需重复标记）                if (grid_[curr_row][curr_col]) {                    continue;                }                // 计算当前栅格与原始障碍物的欧氏距离（单位：栅格）                double dist_grid = std::sqrt(static_cast<double>(dr*dr + dc*dc));                


    
// 距离≤膨胀栅格数 → 标记为膨胀障碍物                if (dist_grid <= inflate_grid_num) {                    grid_[curr_row][curr_col] = true;                    inflated_count++;                }            }        }    }    std::cout  << std::endl;    std::cout  << inflated_count     std::cout  << (original_obstacle_indices.size() + inflated_count) }// // 新增接口：设置默认膨胀半径（支持动态调整）void GridMap2D::setInflateRadius(double radius) {    if (radius <= 0) {        std::cerr  << radius         return;    }    inflate_radius_ = radius;    std::cout  << radius }// 新增接口：使用默认膨胀半径膨胀障碍物（无参调用）void GridMap2D::inflate() {    std::cout  << std::endl;    inflateObstacles(inflate_radius_);  // 复用带参膨胀逻辑}// 世界坐标 → 栅格索引（四舍五入到最近栅格）Eigen::Vector2i GridMap2D::worldToGrid(const Eigen::Vector2d& world_pos) const {    Eigen::Vector2i grid_index;    // 计算逻辑：(世界坐标 - 原点坐标) / 分辨率 → 栅格偏移，四舍五入到整数    grid_index.x() = static_cast<int>(std::round((world_pos.x() - origin_.x()) / resolution_));    grid_index.y() = static_cast<int>(std::round((world_pos.y() - origin_.y()) / resolution_));    // std::cout << "[worldToGrid] 世界坐标（" << world_pos.x() << "," << world_pos.y() << "）→ 栅格索引（"     //           << grid_index.x() << "," << grid_index.y() << "）" << std::endl;    return grid_index;}// 栅格索引 → 世界坐标（返回栅格中心的世界坐标）Eigen::Vector2d GridMap2D::gridToWorld(const Eigen::Vector2i& grid_index) const {    // 校验索引合法性    if (!isIndexValid(grid_index)) {        std::cerr  << grid_index.x() y()                    << std::endl;        return origin_;    }    // 计算逻辑：原点坐标 + 栅格索引 × 分辨率    Eigen::Vector2d world_pos;    world_pos.x() = origin_.x() + grid_index.x() * resolution_;    world_pos.y() = origin_.y() + grid_index.y() * resolution_;    // std::cout << "[gridToWorld] 栅格索引（" << grid_index.x() << "," << grid_index.y() << "）→ 世界坐标（"     //           << world_pos.x() << "," << world_pos.y() << "）" << std::endl;    return world_pos;}// 校验栅格索引是否在地图范围内bool GridMap2D::isIndexValid(const Eigen::Vector2i& grid_index) const {    // 列索引（x）：[0, 列数-1]；行索引（y）：[0, 行数-1]    bool valid = (grid_index.x() >= 0 && grid_index.x() < map_size_.x()) &&                 (grid_index.y() >= 0 && grid_index.y() < map_size_.y());    if (!valid) {        std::cerr  << grid_index.x() y()                    << map_size_.x()-1 y()-1     }    return valid;}


    
// 获取障碍物点云并转化为世界坐标std::vector<:vector2d> GridMap2D::getObstaclePointCloud(bool return_inflated_map) const {    std::vector<:vector2d> point_cloud;    // 预估一下大小以减少内存重分配（可选优化，假设10%是障碍物）    // point_cloud.reserve(map_size_.x() * map_size_.y() * 0.1);    // 遍历整个地图    for (int row = 0; row < map_size_.y(); ++row) {        for (int col = 0; col < map_size_.x(); ++col) {
            // 根据参数决定检查 膨胀网格 还是 原始网格            bool is_obs = return_inflated_map ? grid_[row][col] : original_grid_[row][col];            if (is_obs) {                // 将栅格索引转换为世界坐标                // 逻辑与 gridToWorld 保持一致：原点 + 索引 * 分辨率                double world_x = origin_.x() + col * resolution_;                double world_y = origin_.y() + row * resolution_;                point_cloud.emplace_back(world_x, world_y);            }        }    }    // std::cout << "[getObstaclePointCloud] 生成点云数量：" << point_cloud.size()     //           << " (模式：" << (return_inflated_map ? "膨胀后" : "原始") << ")" << std::endl;    return point_cloud;}// 控制台打印地图（可视化原始/膨胀障碍物）// void GridMap2D::print(bool print_inflated /*= true*/) const {//     std::cout << "\n======================= 地图可视化 =======================" << std::endl;//     std::cout << "  地图类型：" << (print_inflated ? "膨胀后障碍物（#=原始/膨胀，.=自由）" : "原始障碍物（#=原始，.=自由/膨胀）") << std::endl;//     std::cout << "  栅格尺寸：" << map_size_.x() << "列 × " << map_size_.y() << "行" << std::endl;//     std::cout << "==========================================================" << std::endl;//     // 从下到上打印（符合视觉习惯：y轴向上为正）//     for (int row = map_size_.y() - 1; row >= 0; --row) {//         std::cout << "| ";  // 左边界//         for (int col = 0; col < map_size_.x(); ++col) {//             // 选择要打印的网格（原始/膨胀后）//             bool is_occupied = print_inflated ? grid_[row][col] : original_grid_[row][col];//             std::cout << (is_occupied ? "# " : ". ");  // 障碍物→#，自由→.//         }//         std::cout << "|" << std::endl;  // 右边界//     }//     std::cout << "==========================================================" << std::endl;// }// Getter：获取分辨率double GridMap2D::resolution() const {    return resolution_;}// Getter：获取栅格尺寸（列数x行数）const Eigen::Vector2i& GridMap2D::mapSize() const {    return map_size_;}// Getter：获取世界坐标原点const Eigen::Vector2d& GridMap2D::origin() const {    return origin_;}// Getter：获取默认膨胀半径// double GridMap2D::inflateRadius() const {//     return inflate_radius_;// }