00001 #include "obstacle_detection_alg.h" 00002 00003 ObstacleDetectionAlgorithm::ObstacleDetectionAlgorithm(void) 00004 { 00005 } 00006 00007 ObstacleDetectionAlgorithm::~ObstacleDetectionAlgorithm(void) 00008 { 00009 } 00010 00011 void ObstacleDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level) 00012 { 00013 this->lock(); 00014 00015 // save the current configuration 00016 this->config_=new_cfg; 00017 00018 this->unlock(); 00019 } 00020 00021 // ObstacleDetectionAlgorithm Public API 00022 void ObstacleDetectionAlgorithm::CloudConsensusSegmentation(const sensor_msgs::PointCloud2& ros_cloud_in,sensor_msgs::PointCloud2& ros_cloud_out) 00023 { 00024 pcl::PointCloud<pcl::PointXYZ> cloud; 00025 pcl::fromROSMsg (ros_cloud_in, cloud); 00026 pcl::SACSegmentation<pcl::PointXYZRGB> seg; 00027 seg.setOptimizeCoefficients (true); 00028 seg.setModelType (pcl::SACMODEL_PLANE); 00029 seg.setMethodType (pcl::SAC_RANSAC); 00030 seg.setDistanceThreshold (0.01); 00031 seg.setInputCloud(cloud.makeShared()); 00032 pcl::toROSMsg (*seg.getInputCloud(), ros_cloud_out); 00033 } 00034 sensor_msgs::PointCloud2 ObstacleDetectionAlgorithm::getPCLfiltered() 00035 { 00036 sensor_msgs::PointCloud2 out; 00037 CloudConsensusSegmentation(pcl_camera,out); 00038 return out; 00039 } 00040