（11-3-02）基于SLAM的自主路径导航系统：障碍物和边界检测——检测本地障碍物和边界

11.4.2  检测本地障碍物和边界

文件local_rrt_detector.cpp用于检测本地的障碍物和边界，并根据检测结果发布目标点。通过订阅地图和RViz点击点，实现了随机采样、最近点查找、障碍物检测和边界识别等功能，最终发布探索目标和可视化数据，以便机器人在当前位置附近规划安全路径和探索目标。

// 全局变量
nav_msgs::OccupancyGrid mapData; // 存储地图数据
geometry_msgs::PointStamped clickedpoint; // 存储RVIZ中点击的点
geometry_msgs::PointStamped exploration_goal; // 存储探索目标点
visualization_msgs::Marker points, line; // 用于可视化点和线
float xdim, ydim, resolution, Xstartx, Xstarty, init_map_x, init_map_y; // 存储地图尺寸、分辨率和起始点坐标

rdm r; // 生成随机数

// 地图回调函数
void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
    mapData = *msg; // 更新地图数据
}

// RVIZ回调函数
void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) {
    geometry_msgs::Point p;
    p.x = msg->point.x;
    p.y = msg->point.y;
    p.z = msg->point.z;

    points.points.push_back(p); // 将点添加到可视化点数组中
}

// 主函数
int main(int argc, char **argv) {
    // 初始化随机数生成器
    unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7;
    MTRand_int32 irand(init, length);
    MTRand drand; // 已经初始化的双精度随机数生成器

    // 初始化ROS节点
    ros::init(argc, argv, "local_rrt_frontier_detector");
    ros::NodeHandle nh;

    // 获取参数
    float eta, init_map_x, init_map_y, range;
    std::string map_topic, base_frame_topic;

    std::string ns = ros::this_node::getName();

    ros::param::param<float>(ns + "/eta", eta, 0.5);
    ros::param::param<std::string>(ns + "/map_topic", map_topic, "/map");
    ros::param::param<std::string>(ns + "/robot_frame", base_frame_topic, "/base_link");

    // 订阅地图和RVIZ话题
    ros::Subscriber sub = nh.subscribe(map_topic, 100, mapCallBack);
    ros::Subscriber rviz_sub = nh.subscribe("/clicked_point", 100, rvizCallBack);

    // 发布探索目标点和可视化标记
    ros::Publisher targetspub = nh.advertise<geometry_msgs::PointStamped>("/detected_points", 10);
    ros::Publisher pub = nh.advertise<visualization_msgs::Marker>(ns + "_shapes", 10);

    ros::Rate rate(100);

    // 等待地图接收完成
    while (mapData.header.seq < 1 || mapData.data.size() < 1) {
        ros::spinOnce();
        ros::Duration(0.1).sleep();
    }

    // 初始化可视化点和线
    points.header.frame_id = mapData.header.frame_id;
    line.header.frame_id = mapData.header.frame_id;
    points.header.stamp = ros::Time(0);
    line.header.stamp = ros::Time(0);

    points.ns = line.ns = "markers";
    points.id = 0;
    line.id = 1;

    points.type = points.POINTS;
    line.type = line.LINE_LIST;

    // 设置标记动作
    points.action = points.ADD;
    line.action = line.ADD;
    points.pose.orientation.w = 1.0;
    line.pose.orientation.w = 1.0;
    line.scale.x = 0.03;
    line.scale.y = 0.03;
    points.scale.x = 0.3;
    points.scale.y = 0.3;

    line.color.r = 255.0 / 255.0;
    line.color.g = 0.0 / 255.0;
    line.color.b = 0.0 / 255.0;
    points.color.r = 255.0 / 255.0;
    points.color.g = 0.0 / 255.0;
    points.color.b = 0.0 / 255.0;
    points.color.a = 0.3;
    line.color.a = 1.0;
    points.lifetime = ros::Duration();
    line.lifetime = ros::Duration();

    // 初始化点坐标
    geometry_msgs::Point p;

    // 等待至少有5个点被添加到点数组中
    while (points.points.size() < 5) {
        ros::spinOnce();
    }

    // 计算初始地图的中心点
    std::vector<float> temp1, temp2;
    init_map_x = Norm(temp1, temp2); // 计算地图在x轴方向的中心点
    init_map_y = Norm(temp1, temp2); // 计算地图在y轴方向的中心点

    // 初始化探索起点
    Xstartx = (points.points[0].x + points.points[2].x) * 0.5;
    Xstarty = (points.points[0].y + points.points[2].y) * 0.5;

    // 其他初始化代码...

    // 主循环
    while (ros::ok()) {
        // 生成随机点
        std::vector<float> x_rand;
        x_rand.push_back(drand() * init_map_x - init_map_x * 0.5 + Xstartx);
        x_rand.push_back(drand() * init_map_y - init_map_y * 0.5 + Xstarty);

        // 寻找最近的点
        std::vector<float> x_nearest = Nearest(V, x_rand);

        // 转向
        std::vector<float> x_new = Steer(x_nearest, x_rand, eta);

        // 检查路径是否无障碍
        char checking = ObstacleFree(x_nearest, x_new, mapData);

        // 根据检查结果执行相应操作...

        // 发布可视化数据和探索目标点
        pub.publish(points);
        targetspub.publish(exploration_goal);

        // 更新地图和机器人坐标系之间的转换
        tf::StampedTransform transform;
        int temp = 0;
        while (temp == 0) {
            try {
                temp = 1;
                listener.lookupTransform(map_topic, base_frame_topic, ros::Time(0), transform);
            } catch (tf::TransformException ex) {
                temp = 0;
                ros::Duration(0.1).sleep();
            }
        }

        // 更新点坐标...

        // 发布线段可视化
        line.points.clear();
        // 添加点到线段数组中...
        pub.publish(line);

        // 继续执行ROS循环
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

上述代码的主要功能如下所示：

1. 订阅地图和RViz点击点：通过ROS订阅了地图数据和RViz中点击的点。
2. 初始化和设置：对一些全局变量进行了初始化和设置，包括地图数据、点击点、探索目标、可视化标记等。
3. 实现随机采样：使用随机数生成器在地图上进行随机采样，以便搜索可能的探索目标。
4. 实现最近点和扩展操作：实现了在已有点集中查找最近点和进行扩展操作的功能。
5. 检测障碍物和边界：利用地图数据检测到的障碍物，识别出边界区域，确定可探索的前沿区域。
6. 发布探索目标和可视化数据：将识别的前沿点发布为探索目标，并通过ROS发布了用于可视化的标记数据。
7. 利用TF库获取机器人当前位置：使用TF库获取机器人当前位置，并将其作为本地RRT算法的起始点。

通过这些功能，文件实现了本地的RRT路径规划算法，用于在机器人当前位置附近规划安全路径和探索目标。