Go to the documentation of this file.00001 #include "obstacle_detection_normals_alg_node.h"
00002
00003 ObstacleDetectionNormalsAlgNode::ObstacleDetectionNormalsAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<ObstacleDetectionNormalsAlgorithm>(),KSearch(100),normal_z(1),
00005 normal_y(-0.2),normal_x(1),min_dist(0.3)
00006 {
00007
00008
00009
00010
00011 this->obstacles_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("obstacles", 1);
00012 this->all_rg_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("all_rg", 1);
00013
00014
00015 this->input_subscriber_ = this->public_node_handle_.subscribe("input", 1, &ObstacleDetectionNormalsAlgNode::input_callback, this);
00016
00017
00018
00019
00020
00021
00022
00023
00024 }
00025
00026 ObstacleDetectionNormalsAlgNode::~ObstacleDetectionNormalsAlgNode(void)
00027 {
00028
00029 }
00030
00031 void ObstacleDetectionNormalsAlgNode::mainNodeThread(void)
00032 {
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 }
00043
00044
00045 void ObstacleDetectionNormalsAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00046 {
00047
00048
00049
00050 pcl::PointCloud<pcl::PointXYZ> cloud;
00051
00052
00053 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
00054 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_obs(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
00055
00056 pcl::fromROSMsg (*msg, cloud);
00057
00058
00059
00060
00061
00064
00065 this->alg_.lock();
00066
00067 if (!cloud.empty()){
00068 obstacle_detect.cloud_all(KSearch, normal_z, normal_y, normal_x, min_dist, cloud, cloud_out, cloud_obs);
00069 }
00070
00071 this->alg_.unlock();
00072
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 pcl::toROSMsg (*cloud_out, all_rg_msg_);
00086 pcl::toROSMsg (*cloud_obs, obstacles_msg_);
00087
00088 sensor_msgs::convertPointCloud2ToPointCloud(obstacles_msg_, obstaclespc_msg_);
00089
00090 all_rg_msg_.header=msg->header;
00091
00092 this->obstacles_publisher_.publish(this->obstaclespc_msg_);
00093 this->all_rg_publisher_.publish(this->all_rg_msg_);
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105 }
00106
00107
00108
00109
00110
00111
00112
00113 void ObstacleDetectionNormalsAlgNode::node_config_update(Config &config, uint32_t level)
00114 {
00115 this->alg_.lock();
00116 min_dist=config.filter_min_dist;
00117 KSearch=config.normal_KSearch;
00118 normal_z=config.normal_z;
00119 normal_y=config.normal_y;
00120 normal_x=config.normal_x;
00121 this->alg_.unlock();
00122 }
00123
00124 void ObstacleDetectionNormalsAlgNode::addNodeDiagnostics(void)
00125 {
00126 }
00127
00128
00129
00130 int main(int argc,char *argv[])
00131 {
00132 return algorithm_base::main<ObstacleDetectionNormalsAlgNode>(argc, argv, "obstacle_detection_normals_alg_node");
00133 }