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

11.4  障碍物和边界检测

在本项目中，文件global_rrt_detector.cpp和文件local_rrt_detector.cpp实现了ROS节点，用于实现路径规划和障碍物检测功能。它们订阅地图数据和用户点击的点，执行路径规划并生成目标点，然后发布到ROS话题中。通过类似RRT的方法，它们生成路径点，并检测障碍物以避免碰撞，从而实现了障碍物和边界检测的功能。

11.4.1  检测全局障碍物和边界

文件global_rrt_detector.cpp实现了一个 ROS 节点，用于检测全局的障碍物和边界，并根据检测结果发布目标点。通过ROS订阅地图数据和RViz中点击的点来获取地图信息，并使用随机采样最近点（RRT）算法在地图中搜索自由空间和边界。然后，它根据检测到的结果发布检测到的边界点，以及下一步的探索目标点，以便进行路径规划或其他后续操作。

// 全局变量
nav_msgs::OccupancyGrid mapData;
geometry_msgs::PointStamped clickedpoint;
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;
}

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); // 32位整数生成器
    MTRand drand; // [0, 1)之间的双精度浮点数生成器，已初始化

    ros::init(argc, argv, "global_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;
    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::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); 

    // 等待地图接收，当接收到地图时，mapData.header.seq将不会小于1
    while (mapData.header.seq < 1 or 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 = 11.0/255.0;
    line.color.g = 91.0/255.0;
    line.color.b = 236.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 = 1.0;
    line.color.a = 1.0;
    points.lifetime = ros::Duration();
    line.lifetime = ros::Duration();
    geometry_msgs::Point p;  

    // 发布点直到有足够的点
    while (points.points.size() < 5) {
        ros::spinOnce();
        pub.publish(points);
    }

    std::vector<float> temp1;
    temp1.push_back(points.points[0].x);
    temp1.push_back(points.points[0].y);
    
    std::vector<float> temp2; 
    temp2.push_back(points.points[2].x);
    temp2.push_back(points.points[0].y);

    init_map_x = Norm(temp1, temp2);
    temp1.clear();
    temp2.clear();

    temp1.push_back(points.points[0].x);
    temp1.push_back(points.points[0].y);

    temp2.push_back(points.points[0].x);
    temp2.push_back(points.points[2].y);

    init_map_y = Norm(temp1, temp2);
    temp1.clear();
    temp2.clear();

    Xstartx = (points.points[0].x + points.points[2].x) * 0.5;
    Xstarty = (points.points[0].y + points.points[2].y) * 0.5;

    geometry_msgs::Point trans;
    trans = points.points[4];
    std::vector< std::vector<float>  > V; 
    std::vector<float> xnew; 
    xnew.push_back(trans.x);
    xnew.push_back(trans.y);  
    V.push_back(xnew);

    points.points.clear();
    pub.publish(points);

    std::vector<float> frontiers;
    int i = 0;
    float xr, yr;
    std::vector<float> x_rand, x_nearest, x_new;

    // 主循环
    while (ros::ok()) {
        // 采样自由空间
        x_rand.clear();
        xr = (drand() * init_map_x) - (init_map_x * 0.5) + Xstartx;
        yr = (drand() * init_map_y) - (init_map_y * 0.5) + Xstarty;
        x_rand.push_back(xr);
        x_rand.push_back(yr);

        // 最近点
        x_nearest = Nearest(V, x_rand);

        // Steer
        x_new = Steer(x_nearest, x_rand, eta);

        // 检查障碍物
        char checking = ObstacleFree(x_nearest, x_new, mapData);

        if (checking == -1) {
            exploration_goal.header.stamp = ros::Time(0);
            exploration_goal.header.frame_id = mapData.header.frame_id;
            exploration_goal.point.x = x_new[0];
            exploration_goal.point.y = x_new[1];
            exploration_goal.point.z = 0.0;
            p.x = x_new[0]; 
            p.y = x_new[1]; 
            p.z = 0.0;
            points.points.push_back(p);
            pub.publish(points);
            targetspub.publish(exploration_goal);
            points.points.clear();
        } else if (checking == 1) {
            V.push_back(x_new);
            p.x = x_new[0]; 
            p.y = x_new[1]; 
            p.z = 0.0;
            line.points.push_back(p);
            p.x = x_nearest[0]; 
            p.y = x_nearest[1]; 
            p.z = 0.0;
            line.points.push_back(p);
        }

        pub.publish(line);  
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

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

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

通过这些功能，文件实现了全局的障碍物和边界检测，以便在机器人探索环境时确定安全路径和探索目标。