Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _obstacle_detection_alg_h_
00026 #define _obstacle_detection_alg_h_
00027 #include <pcl/sample_consensus/model_types.h>
00028 #include <pcl/sample_consensus/method_types.h>
00029 #include <pcl/segmentation/sac_segmentation.h>
00030 #include <pcl/point_cloud.h>
00031 #include <pcl/point_types.h>
00032 #include <pcl/ros/conversions.h>
00033 #include <iri_obstacle_detection/ObstacleDetectionConfig.h>
00034 #include <sensor_msgs/PointCloud2.h>
00035 #include "mutex.h"
00036
00037
00038
00044 class ObstacleDetectionAlgorithm
00045 {
00046 protected:
00053 CMutex alg_mutex_;
00054
00055
00056
00057 public:
00064 typedef iri_obstacle_detection::ObstacleDetectionConfig Config;
00065
00072 Config config_;
00073
00082 ObstacleDetectionAlgorithm(void);
00083
00089 void lock(void) { alg_mutex_.enter(); };
00090
00096 void unlock(void) { alg_mutex_.exit(); };
00097
00105 bool try_enter(void) { return alg_mutex_.try_enter(); };
00106
00118 void config_update(Config& new_cfg, uint32_t level=0);
00119
00120
00121
00122
00129 ~ObstacleDetectionAlgorithm(void);
00130
00131 sensor_msgs::PointCloud2 pcl_camera;
00132
00133 void CloudConsensusSegmentation(const sensor_msgs::PointCloud2& ros_cloud_in,sensor_msgs::PointCloud2& ros_cloud_out);
00134 sensor_msgs::PointCloud2 getPCLfiltered();
00135 };
00136
00137 #endif