在egoplanner中设置一片禁止通行区

背景：想在ego的基础上写一个自主探索无人机，限于现在对玻璃还未处理好，先对大范围玻璃等不可识别区域提前标出不可通行。

思路：直接在ego的grid_map里点云。

操作：

1:在grid_map.h里添加头文件

我加在186行

// Mark the forbidden path
  pcl::PointCloud<pcl::PointXYZ>  set_no_path(double x_start, double y_start,double z_start,double x_end, double y_end,double z_end,double resolution_);
  

具体实现：398

 

inline pcl::PointCloud<pcl::PointXYZ>  GridMap::set_no_path(double x_start, double y_start,double z_start,double x_end, double y_end,double z_end,double resolution_){
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_path(new pcl::PointCloud<pcl::PointXYZ>);
     if(x_start>x_end)swap(x_start,x_end);
     if(y_start>y_end)swap(y_start,y_end);
     if(z_start>z_end)swap(z_start,z_end);
     x_end+=resolution_;y_end+=resolution_;z_end+=resolution_;
    cout<<"grid_map.h x_start: "<<x_start<<" x_end:"<<x_end<<endl;

    cloud_no_path->points.resize(ceil(abs(x_end-x_start)*(abs(y_end-y_start)*(abs(z_end-z_start)/resolution_)/resolution_)/resolution_));
    cout<<"grid_map.h cloud_no_path->points.size::"<<cloud_no_path->points.size()<<endl;

    int i=0;
    for(double j = x_start;j<x_end;j+=resolution_){
         cout<<"xstart"<<x_start<<"xend"<<x_end<<"resolution"<<resolution_<<endl;
      for(double k = y_start;k<y_end;k+=resolution_){
          cout<<"ystart"<<y_start<<"yend"<<y_end<<"resolution"<<resolution_<<endl; 
        for(double l = z_start;l<z_end;l+=resolution_){
          cout<<"zstart"<<z_start<<"zend"<<z_end<<"resolution"<<resolution_<<endl;
              cloud_no_path->points[i].x=j;
              cloud_no_path->points[i].y=k;
              cloud_no_path->points[i].z=l;
              i++;
              }
         }
      
 cout<<"grid_map.h cloud_no_path->points.size::"<<cloud_no_path->points.size()<<endl;
    }
    return *cloud_no_path;  

}

在需要的地方就可以直接设置上去了。比如初始化的地方建立一个，然后每次回调加上这个屏障。