**判断三维激光点云前方障碍物**

判断三维激光点云前方障碍物

我完成的是这样的功能判断前方20m范围内x，y,z分别限定在一定范围，判断一定范围内点云的数量来判断是否有障碍物

#代码`#include
#include “ros/ros.h”
#include “sensor_msgs/LaserScan.h”
#include <boost/foreach.hpp>
#include “std_msgs/Bool.h”
#include “laser_handle/pub_sub_picPoint2cloud.hpp”
#include <math.h>

#include <pcl/filters/passthrough.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/radius_outlier_removal.h>

#include <pcl/point_cloud.h>

pcl::visualization::CloudViewer viewer(“Cloud Viewer”);
ros::Subscriber rslidar_cloud;
ros::Publisher cloud;
ros::Publisher isObstacle;
typedef pcl::PointCloudpcl::PointXYZ PointCloud;
pcl::PointCloudpcl::PointXYZ::Ptr cloud_msg (new pcl::PointCloudpcl::PointXYZ);
pcl::PointCloudpcl::PointXYZ::Ptr cloud_radiusInclude (new pcl::PointCloudpcl::PointXYZ);
pcl::PointCloudpcl::PointXYZ::Ptr cloud_frontInclude (new pcl::PointCloudpcl::PointXYZ);

sensor_msgs::PointCloud2 obstacle;
int obstacleFlag = 1000;
int radius = 20;//扫描半径
float x_min = -2;
float x_max = 2;
float y_min = 0;
float y_max = 22;
float z_min = 0;
float z_max = 1;

void scan_pcl(sensor_msgs::PointCloud2 &scan_msg){
pcl::PointCloudpcl::PointXYZ::Ptr cloud_filted_height (new pcl::PointCloudpcl::PointXYZ);
pcl::PointCloudpcl::PointXYZ::Ptr cloud_filted_radius (new pcl::PointCloudpcl::PointXYZ);
pcl::PointCloudpcl::PointXYZ::Ptr cloud_filted_front (new pcl::PointCloudpcl::PointXYZ);

cloud_msg->clear();
cloud_radiusInclude->clear();
cloud_frontInclude->clear();

int point_radius;
pcl::fromROSMsg(scan_msg,*cloud_msg);
cloud_msg->header.frame_id = "laser";

std::cerr<<"pclcloud size is"<<cloud_msg->size()<<std::endl;

    // Create the filtering object
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud_msg);
pass.setFilterFieldName ("z");
pass.setFilterLimits (z_min, z_max);//滤掉超过指定范围之外的值
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filted_height);

pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
// build the filter
outrem.setInputCloud(cloud_filted_height);
outrem.setRadiusSearch(0.8);
outrem.setMinNeighborsInRadius (2);
outrem.setKeepOrganized(true);
// apply filter
outrem.filter (*cloud_filted_radius);

std::cerr << "Cloud after filterZ: " << cloud_filted_radius->size()<<std::endl;
//viewer.showCloud(cloud_filted_radius);


pcl::PointCloud<pcl::PointXYZ>::iterator index = cloud_radiusInclude->begin();
for (auto point:*cloud_filted_radius){
    point_radius= sqrt(pow(point.x,2)*pow(point.y,2)*pow(point.z,2));
    if (point_radius<=radius){   
        cloud_radiusInclude->push_back(point);                  
    }
    
}

pass.setInputCloud (cloud_radiusInclude);
pass.setFilterFieldName ("x");
pass.setFilterLimits (x_min, x_max);//滤掉超过指定范围之外的值
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filted_front); 
std::cerr << "Cloud after filterZ X: " << cloud_filted_front->size()<<std::endl;

pass.setInputCloud (cloud_filted_front);
pass.setFilterFieldName ("y");
pass.setFilterLimits (y_min, y_max);//滤掉超过指定范围之外的值
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_frontInclude);
std::cerr << "Cloud after filterZ X Y: " << cloud_frontInclude->size()<<std::endl; 

int pointnumber = cloud_frontInclude->size();
std_msgs::Bool msg;
if (pointnumber < obstacleFlag){
    msg.data = false;
}
else{
    msg.data = true;
}    
std::cerr << "obstancle: " << msg.data<<std::endl; 
viewer.showCloud(cloud_frontInclude);
pcl::toROSMsg(*cloud_frontInclude,obstacle);
obstacle.header = scan_msg.header;
cloud.publish(obstacle);

isObstacle.publish(msg);

}

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg){
sensor_msgs::PointCloud2 output= *msg;
ros::NodeHandle nh;
std::cerr<<“z_min,z_max,x_min.x_max,y_min,y_max”<<std::endl;
scan_pcl(output);

}
int main(int argc,char * argv[])
{

ros::init(argc, argv, "scan_pcl");
ros::NodeHandle nh（"~"）;
  if(nh.getParam("z_min",z_min)&nh.getParam("z_max",z_max)&nh.getParam("x_min",x_min)&\
nh.getParam("x_max",x_max)&nh.getParam("z_min",z_min)&nh.getParam("z_max",z_max)&nh.getParam("obstacleFlag",obstacleFlag)){
    std::cerr<<"z_min,z_max,x_min.x_max,y_min,y_max"<<std::endl;
}

rslidar_cloud = nh.subscribe("/rslidar_points", 10, cloudCallback);
cloud=nh.advertise<sensor_msgs::PointCloud2>("/obstacle_cloud",10);
isObstacle = nh.advertise<std_msgs::Bool>("/isObstacle_cloud",10);



ros::spin();
return 0;

}`
launch 文件

<launch>
<node name = "is_obstacle" pkg = "laser_handle" type ="pub_sub_picPoint2cloud">
  <param name="z_min" value ="0"/>
  <param name="z_max" value="2"/>
  <param name="x_min" value="-2"/>
  <param name="x_max" value="2"/>
  <param name="y_min" value="0"/>
  <param name="y_max" value="20"/>
  <param name="obstacleFlag" value="1000"/>
</node>
</launch> 

简单说两句参数服务器；
在launch文件中node写的参数就算定义了比如就是定义在局部变量下，在终端里面用rosparam打印参数是这样的/is_obstacle/z_min。若是在node外就是全局定义在终端上打印出来就是/z_min。还有就算nh句柄初始化时带～如handle nh("~")就是私有化的句柄，访问的变量默认带了前缀。