$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: detector.cpp 777 2012-05-11 11:23:17Z ihulik $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Rostislav Hulik (ihulik@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 11.01.2012 (version 0.8) 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00037 #include <ros/ros.h> 00038 #include <image_transport/image_transport.h> 00039 #include <cv_bridge/cv_bridge.h> 00040 00041 // OpenCV 2 00042 #include <opencv2/highgui/highgui.hpp> 00043 #include <opencv2/imgproc/imgproc_c.h> 00044 00045 #include <sensor_msgs/Image.h> 00046 #include <sensor_msgs/CameraInfo.h> 00047 #include <visualization_msgs/Marker.h> 00048 #include <visualization_msgs/MarkerArray.h> 00049 #include <boost/math/quaternion.hpp> 00050 00051 #include <tf/transform_listener.h> 00052 #include <tf/message_filter.h> 00053 #include <tf/tfMessage.h> 00054 00055 #include <message_filters/subscriber.h> 00056 #include <message_filters/time_synchronizer.h> 00057 #include <message_filters/synchronizer.h> 00058 #include <message_filters/sync_policies/approximate_time.h> 00059 00060 #include <srs_env_model_percp/ClearPlanes.h> 00061 00062 #include <srs_env_model_percp/but_segmentation/filtering.h> 00063 #include <srs_env_model_percp/but_segmentation/normals.h> 00064 00065 #include <srs_env_model_percp/topics_list.h> 00066 #include <srs_env_model_percp/services_list.h> 00067 00068 #include <srs_env_model_percp/but_plane_detector/scene_model.h> 00069 #include <srs_env_model_percp/but_plane_detector/dyn_model_exporter.h> 00070 #include <srs_env_model_percp/but_plane_detector/plane_detector_params.h> 00071 00072 // PCL 00073 #include <pcl/point_types.h> 00074 #include <pcl/point_cloud.h> 00075 #include <pcl/io/pcd_io.h> 00076 00077 #include <pcl/kdtree/kdtree_flann.h> 00078 #include <pcl/features/normal_3d.h> 00079 #include <pcl/surface/gp3.h> 00080 #include <pcl/io/vtk_io.h> 00081 #include <pcl/filters/extract_indices.h> 00082 #include <pcl/filters/voxel_grid.h> 00083 #include <pcl/ModelCoefficients.h> 00084 #include <pcl/io/pcd_io.h> 00085 #include <pcl/point_types.h> 00086 #include <pcl/sample_consensus/method_types.h> 00087 #include <pcl/sample_consensus/model_types.h> 00088 #include <pcl/segmentation/sac_segmentation.h> 00089 00090 #include <pcl_ros/point_cloud.h> 00091 #include <pcl_ros/transforms.h> 00092 00093 #include <cob_3d_mapping_msgs/Shape.h> 00094 #include <cob_3d_mapping_msgs/ShapeArray.h> 00095 00096 00097 #include <cfloat> 00098 #include <cstdlib> 00099 #include <cstdio> 00100 #include <iostream> 00101 00102 #include <srs_env_model_percp/LoadSave.h> 00103 #include <srs_env_model_percp/ResetPlanes.h> 00104 00105 using namespace std; 00106 using namespace cv; 00107 using namespace pcl; 00108 using namespace sensor_msgs; 00109 using namespace message_filters; 00110 using namespace but_plane_detector; 00111 typedef but_plane_detector::Plane<float> tPlane; 00112 typedef std::vector<tPlane, Eigen::aligned_allocator<tPlane> > tPlanes; 00113 00114 namespace srs_env_model_percp 00115 { 00116 int counter = 0; 00117 sensor_msgs::PointCloud2 cloud_msg; 00118 tf::MessageFilter<sensor_msgs::PointCloud2> *transform_filter; 00119 tf::TransformListener *tfListener; 00120 ros::Publisher pub1; 00121 ros::Publisher pub2; 00122 ros::Publisher pub3; 00123 DynModelExporter *exporter = NULL; 00124 00125 sensor_msgs::CameraInfo cam_info_legacy; 00126 sensor_msgs::CameraInfoConstPtr cam_info_aux (&cam_info_legacy); 00127 00128 PlaneDetectorSettings settings; 00129 00130 ros::NodeHandle *n; 00134 void callbackpcl(const sensor_msgs::PointCloud2ConstPtr& cloud); 00135 void callbackkinect( const sensor_msgs::ImageConstPtr& dep, const sensor_msgs::CameraInfoConstPtr& cam_info); 00136 bool clear(srs_env_model_percp::ClearPlanes::Request &req,srs_env_model_percp::ClearPlanes::Response &res); 00137 bool getParams(ros::NodeHandle nh); 00138 00139 void pcl_process(const PointCloud2ConstPtr& cloud, const sensor_msgs::ImageConstPtr* rgb = NULL) 00140 { 00141 ++counter; 00142 ros::Time begin = ros::Time::now(); 00143 00144 // make the pointcloud 00145 pcl::PointCloud<pcl::PointXYZ> pointcloud; 00146 pcl::fromROSMsg (*cloud, pointcloud); 00147 00148 // Control out 00149 std::cerr << "Recieved frame..." << std::endl; 00150 std::cerr << "Topic: " << pointcloud.header.frame_id << std::endl; 00151 std::cerr << "Width: " << pointcloud.width << " height: " << pointcloud.height << std::endl; 00152 std::cerr << "=========================================================" << endl; 00153 00154 // get indices of points to be deleted (max depth) 00155 std::vector<unsigned int> zero_indices; 00156 for (unsigned int i = 0; i < pointcloud.points.size(); ++i) 00157 { 00158 if (pointcloud.points[i].z > settings.param_ht_maxdepth) 00159 zero_indices.push_back(i); 00160 } 00161 00162 // transform to world 00163 tf::StampedTransform sensorToWorldTf; 00164 try { 00165 tfListener->waitForTransform(settings.param_output_frame, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2)); 00166 tfListener->lookupTransform(settings.param_output_frame, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); 00167 } 00168 catch(tf::TransformException& ex){ 00169 std::cerr << "Transform error: " << ex.what() << ", quitting callback" << std::endl; 00170 return; 00171 } 00172 00173 // transform pointcloud from sensor frame to fixed robot frame 00174 Eigen::Matrix4f sensorToWorld; 00175 pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); 00176 pcl::transformPointCloud(pointcloud, pointcloud, sensorToWorld); 00177 00178 for (unsigned int i = 0; i < pointcloud.points.size(); ++i) 00179 { 00180 if (pointcloud.points[i].z > settings.param_ht_maxheight || pointcloud.points[i].z < settings.param_ht_minheight) 00181 zero_indices.push_back(i); 00182 } 00183 00184 // set all zero indices to point(0,0,0) 00185 for (unsigned int i = 0; i < zero_indices.size(); ++i) 00186 { 00187 pointcloud.points[zero_indices[i]].x = 0.0; 00188 pointcloud.points[zero_indices[i]].y = 0.0; 00189 pointcloud.points[zero_indices[i]].z = 0.0; 00190 } 00191 00192 Normals normal(pointcloud); 00193 00195 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 00196 pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 00197 // Create the segmentation object 00198 pcl::SACSegmentation<pcl::PointXYZ> seg; 00199 00200 pointcloud.header.frame_id = settings.param_output_frame; 00201 00202 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPtr(new pcl::PointCloud<pcl::PointXYZ>(pointcloud)); 00203 pcl::PointCloud<pcl::PointXYZ>::Ptr auxPtr(new pcl::PointCloud<pcl::PointXYZ>); 00204 00205 // Optional 00206 seg.setOptimizeCoefficients (true); 00207 // Mandatory 00208 seg.setModelType (pcl::SACMODEL_PLANE); 00209 seg.setMethodType (pcl::SAC_RANSAC); 00210 00211 seg.setDistanceThreshold (settings.param_visualisation_distance); 00212 00213 seg.setInputCloud (cloudPtr); 00214 00215 seg.segment (*inliers, *coefficients); 00216 tPlanes planes; 00217 int id = 1; 00218 00219 while ((int)inliers->indices.size () > settings.param_visualisation_min_count) 00220 { 00221 ++id; 00222 std::cerr << inliers->indices.size() << std::endl; 00223 pcl::ExtractIndices<pcl::PointXYZ> extract; 00224 extract.setInputCloud (cloudPtr); 00225 extract.setIndices (inliers); 00226 extract.setNegative (true); 00227 extract.filter (*auxPtr); 00228 std::vector<cv::Vec3f> points; 00229 00230 //Plane<float> LeastSquaresPlane(std::vector<cv::Vec3f> &points); 00231 for (size_t i = 0; i < inliers->indices.size (); ++i) 00232 { 00233 cv::Vec3f current( cloudPtr->points[inliers->indices[i]].x, 00234 cloudPtr->points[inliers->indices[i]].y, 00235 cloudPtr->points[inliers->indices[i]].z); 00236 points.push_back(current); 00237 } 00238 planes.push_back(Normals::LeastSquaresPlane(points)); 00239 00240 cloudPtr->clear(); 00241 pcl::copyPointCloud(*auxPtr, *cloudPtr); 00242 inliers->indices.clear(); 00243 seg.segment (*inliers, *coefficients); 00244 } 00245 // update planes on server 00246 //exporter->update(planes, normal, sensorToWorldTf); 00247 // update current sent planes 00248 if (rgb && settings.param_visualisation_color == "mean_color") 00249 { 00250 cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(*rgb); 00251 cv::Mat rgb_mat = cv_image->image; 00252 exporter->update(planes, normal, settings.param_visualisation_color, rgb_mat); 00253 } 00254 else 00255 { 00256 exporter->update(planes, normal, settings.param_visualisation_color); 00257 } 00258 00259 visualization_msgs::MarkerArray marker_array; 00260 cob_3d_mapping_msgs::ShapeArray shape_array; 00261 exporter->getMarkerArray(marker_array, settings.param_output_frame); 00262 exporter->getShapeArray(shape_array, settings.param_output_frame); 00263 std::cerr << "Total no of sent planes: " << marker_array.markers.size() << std::endl; 00264 pub2.publish(marker_array); 00265 pub3.publish(shape_array); 00266 00267 // Control out 00268 ros::Time end = ros::Time::now(); 00269 std::cerr << "DONE.... Computation time: " << (end-begin).nsec/1000000.0 << " ms." << std::endl; 00270 std::cerr << "=========================================================" << endl<< endl; 00271 } 00275 void callbackpcl_rgb(const PointCloud2ConstPtr& cloud, const sensor_msgs::ImageConstPtr& rgb) 00276 { 00277 pcl_process(cloud, &rgb); 00278 } 00279 00280 void callbackpcl(const PointCloud2ConstPtr& cloud) 00281 { 00282 pcl_process(cloud); 00283 } 00284 00285 00286 void callbackkinect( const sensor_msgs::ImageConstPtr& dep, const CameraInfoConstPtr& cam_info) 00287 { 00288 ++counter; 00289 ros::Time begin = ros::Time::now(); 00290 00291 // Control out 00292 std::cerr << "Kinect input not ready yet.... doing nothing." << std::endl; 00293 ros::Time end = ros::Time::now(); 00294 std::cerr << "DONE.... Computation time: " << (end-begin).nsec/1000000.0 << " ms." << std::endl; 00295 std::cerr << "=========================================================" << endl<< endl; 00296 } 00297 00298 bool getParams(ros::NodeHandle nh) 00299 { 00300 nh.param(PARAM_NODE_INPUT, settings.param_node_input, PARAM_NODE_INPUT_DEFAULT); 00301 00302 nh.param(PARAM_NODE_OUTPUT_FRAME, settings.param_output_frame, PARAM_NODE_OUTPUT_FRAME_DEFAULT); 00303 nh.param(PARAM_NODE_ORIGINAL_FRAME, settings.param_original_frame, PARAM_NODE_ORIGINAL_FRAME_DEFAULT); 00304 00305 nh.param(PARAM_HT_KEEPTRACK, settings.param_ht_keeptrack, PARAM_HT_KEEPTRACK_DEFAULT); 00306 nh.param(PARAM_HT_MAXDEPTH, settings.param_ht_maxdepth, PARAM_HT_MAXDEPTH_DEFAULT); 00307 nh.param(PARAM_HT_MAXHEIGHT, settings.param_ht_maxheight, PARAM_HT_MAXHEIGHT_DEFAULT); 00308 nh.param(PARAM_HT_MINHEIGHT, settings.param_ht_minheight, PARAM_HT_MINHEIGHT_DEFAULT); 00309 00310 nh.param(PARAM_HT_MINSHIFT, settings.param_ht_minshift, PARAM_HT_MINSHIFT_DEFAULT); 00311 nh.param(PARAM_HT_MAXSHIFT, settings.param_ht_maxshift, PARAM_HT_MAXSHIFT_DEFAULT); 00312 nh.param(PARAM_HT_ANGLE_RES, settings.param_ht_angle_res, PARAM_HT_ANGLE_RES_DEFAULT); 00313 nh.param(PARAM_HT_SHIFT_RES, settings.param_ht_shift_res, PARAM_HT_SHIFT_RES_DEFAULT); 00314 nh.param(PARAM_HT_GAUSS_ANGLE_RES, settings.param_ht_gauss_angle_res, PARAM_HT_GAUSS_ANGLE_RES_DEFAULT); 00315 nh.param(PARAM_HT_GAUSS_SHIFT_RES, settings.param_ht_gauss_shift_res, PARAM_HT_GAUSS_SHIFT_RES_DEFAULT); 00316 nh.param(PARAM_HT_GAUSS_ANGLE_SIGMA, settings.param_ht_gauss_angle_sigma, PARAM_HT_GAUSS_ANGLE_SIGMA_DEFAULT); 00317 nh.param(PARAM_HT_GAUSS_SHIFT_SIGMA, settings.param_ht_gauss_shift_sigma, PARAM_HT_GAUSS_SHIFT_SIGMA_DEFAULT); 00318 nh.param(PARAM_HT_LVL1_GAUSS_ANGLE_RES, settings.param_ht_lvl1_gauss_angle_res, PARAM_HT_LVL1_GAUSS_ANGLE_RES_DEFAULT); 00319 nh.param(PARAM_HT_LVL1_GAUSS_SHIFT_RES, settings.param_ht_lvl1_gauss_shift_res, PARAM_HT_LVL1_GAUSS_SHIFT_RES_DEFAULT); 00320 nh.param(PARAM_HT_LVL1_GAUSS_ANGLE_SIGMA, settings.param_ht_lvl1_gauss_angle_sigma, PARAM_HT_LVL1_GAUSS_ANGLE_SIGMA_DEFAULT); 00321 nh.param(PARAM_HT_LVL1_GAUSS_SHIFT_SIGMA, settings.param_ht_lvl1_gauss_shift_sigma, PARAM_HT_LVL1_GAUSS_SHIFT_SIGMA_DEFAULT); 00322 nh.param(PARAM_HT_MIN_SMOOTH, settings.param_ht_min_smooth, PARAM_HT_MIN_SMOOTH_DEFAULT); 00323 00324 nh.param(PARAM_HT_PLANE_MERGE_ANGLE, settings.param_ht_plane_merge_angle, PARAM_HT_PLANE_MERGE_ANGLE_DEFAULT); 00325 nh.param(PARAM_HT_PLANE_MERGE_SHIFT, settings.param_ht_plane_merge_shift, PARAM_HT_PLANE_MERGE_SHIFT_DEFAULT); 00326 00327 nh.param(PARAM_VISUALISATION_DISTANCE, settings.param_visualisation_distance, PARAM_VISUALISATION_DISTANCE_DEFAULT); 00328 nh.param(PARAM_VISUALISATION_PLANE_NORMAL_DEV, settings.param_visualisation_plane_normal_dev, PARAM_VISUALISATION_PLANE_NORMAL_DEV_DEFAULT); 00329 nh.param(PARAM_VISUALISATION_PLANE_SHIFT_DEV, settings.param_visualisation_plane_shift_dev, PARAM_VISUALISATION_PLANE_SHIFT_DEV_DEFAULT); 00330 nh.param(PARAM_VISUALISATION_MIN_COUNT, settings.param_visualisation_min_count, PARAM_VISUALISATION_MIN_COUNT_DEFAULT); 00331 nh.param(PARAM_VISUALISATION_COLOR, settings.param_visualisation_color, PARAM_VISUALISATION_COLOR_DEFAULT); 00332 nh.param(PARAM_VISUALISATION_TTL, settings.param_visualisation_ttl, PARAM_VISUALISATION_TTL_DEFAULT); 00333 nh.param(PARAM_VISUALISATION_MAX_POLY_SIZE, settings.param_visualisation_max_poly_size, PARAM_VISUALISATION_MAX_POLY_SIZE_DEFAULT); 00334 00335 nh.param(PARAM_SEARCH_MINIMUM_CURRENT_SPACE, settings.param_search_minimum_current_space, PARAM_SEARCH_MINIMUM_CURRENT_SPACE_DEFAULT); 00336 nh.param(PARAM_SEARCH_MINIMUM_GLOBAL_SPACE, settings.param_search_minimum_global_space, PARAM_SEARCH_MINIMUM_GLOBAL_SPACE_DEFAULT); 00337 nh.param(PARAM_SEARCH_MAXIMA_SEARCH_NEIGHBORHOOD, settings.param_search_maxima_search_neighborhood, PARAM_SEARCH_MAXIMA_SEARCH_NEIGHBORHOOD_DEFAULT); 00338 nh.param(PARAM_SEARCH_MAXIMA_SEARCH_BLUR, settings.param_search_maxima_search_blur, PARAM_SEARCH_MAXIMA_SEARCH_BLUR_DEFAULT); 00339 00340 return true; 00341 } 00342 00343 bool onReset(srs_env_model_percp::ResetPlanes::Request &req, srs_env_model_percp::ResetPlanes::Response &res) 00344 { 00345 std::cout << "Resetting plane settings..." << std::endl; 00346 00347 visualization_msgs::MarkerArray marker_array; 00348 exporter->getMarkerArray(marker_array, settings.param_output_frame); 00349 00350 for (unsigned int i = 0; i < marker_array.markers.size(); ++i) 00351 marker_array.markers[i].action = visualization_msgs::Marker::DELETE; 00352 00353 std::cerr << "Total no of deleted planes: " << marker_array.markers.size() << std::endl; 00354 pub2.publish(marker_array); 00355 00356 delete exporter; 00357 exporter = new DynModelExporter(n, 00358 settings.param_original_frame, 00359 settings.param_output_frame, 00360 settings.param_visualisation_min_count, 00361 settings.param_visualisation_distance, 00362 settings.param_visualisation_plane_normal_dev, 00363 settings.param_visualisation_plane_shift_dev, 00364 settings.param_ht_keeptrack, 00365 settings.param_visualisation_ttl, 00366 settings.param_visualisation_max_poly_size); 00367 00368 res.message = "Hough space successfully reset.\n"; 00369 std::cout << "Hough space successfully reset." << std::endl; 00370 return true; 00371 } 00372 00373 bool onSave(srs_env_model_percp::LoadSave::Request &req, srs_env_model_percp::LoadSave::Response &res) 00374 { 00375 std::cerr << "Saving planes...." << std::endl; 00376 exporter->xmlFileExport(req.filename); 00377 00378 00379 res.all_ok = 1; 00380 std::cerr << "Environment model successfuly saved into " << req.filename << "." << std::endl; 00381 return true; 00382 } 00383 00384 bool onLoad(srs_env_model_percp::LoadSave::Request &req, srs_env_model_percp::LoadSave::Response &res) 00385 { 00386 std::cerr << "Loading planes...." << std::endl; 00387 00388 visualization_msgs::MarkerArray marker_array; 00389 exporter->getMarkerArray(marker_array, settings.param_output_frame); 00390 00391 for (unsigned int i = 0; i < marker_array.markers.size(); ++i) 00392 marker_array.markers[i].action = visualization_msgs::Marker::DELETE; 00393 00394 std::cerr << "Total no of deleted planes: " << marker_array.markers.size() << std::endl; 00395 pub2.publish(marker_array); 00396 00397 delete exporter; 00398 exporter = new DynModelExporter(n, 00399 settings.param_original_frame, 00400 settings.param_output_frame, 00401 settings.param_visualisation_min_count, 00402 settings.param_visualisation_distance, 00403 settings.param_visualisation_plane_normal_dev, 00404 settings.param_visualisation_plane_shift_dev, 00405 settings.param_ht_keeptrack, 00406 settings.param_visualisation_ttl, 00407 settings.param_visualisation_max_poly_size); 00408 00409 exporter->xmlFileImport(req.filename); 00410 00411 marker_array.markers.clear(); 00412 cob_3d_mapping_msgs::ShapeArray shape_array; 00413 exporter->getMarkerArray(marker_array, settings.param_output_frame); 00414 exporter->getShapeArray(shape_array, settings.param_output_frame); 00415 std::cerr << "Total no of sent planes: " << marker_array.markers.size() << std::endl; 00416 pub2.publish(marker_array); 00417 pub3.publish(shape_array); 00418 00419 res.all_ok = 1; 00420 std::cerr << "Environment model successfuly loaded from " << req.filename << "." << std::endl; 00421 return true; 00422 } 00423 } 00424 00425 00429 int main( int argc, char** argv ) 00430 { 00431 using namespace srs_env_model_percp; 00432 00433 ros::init(argc, argv, "but_plane_detector_ransac"); 00434 n = new ros::NodeHandle(); 00435 00436 // Private node handle to read private node parameters 00437 ros::NodeHandle private_nh("~"); 00438 getParams(private_nh); 00439 00440 //ros::ServiceServer service = n.advertiseService(DET_SERVICE_CLEAR_PLANES, clear); 00441 00442 // Print out parameters 00443 std::cerr << std::endl; 00444 std::cerr << "Given HS parameters:" << std::endl; 00445 std::cerr << "====================" << std::endl; 00446 std::cerr << "Max point cloud z value: " << settings.param_ht_maxdepth << std::endl; 00447 std::cerr << "Minimal plane shift (d param) value: " << settings.param_ht_minshift << std::endl; 00448 std::cerr << "Maximal plane shift (d param) value: " << settings.param_ht_maxshift << std::endl; 00449 std::cerr << "Hough space angle resolution: " << settings.param_ht_angle_res << std::endl; 00450 std::cerr << "Hough space shift resolution: " << settings.param_ht_shift_res << std::endl; 00451 std::cerr << "Plane Gauss function angle resolution: " << settings.param_ht_gauss_angle_res << std::endl; 00452 std::cerr << "Plane Gauss function shift resolution: " << settings.param_ht_gauss_shift_res << std::endl; 00453 std::cerr << "Plane Gauss function angle sigma: " << settings.param_ht_gauss_angle_sigma << std::endl; 00454 std::cerr << "Plane Gauss function shift sigma: " << settings.param_ht_gauss_shift_sigma << std::endl; 00455 std::cerr << std::endl; 00456 std::cerr << "Given plane search parameters:" << std::endl; 00457 std::cerr << "==============================" << std::endl; 00458 std::cerr << "Minimum plane value in HS for current frame: " << settings.param_search_minimum_current_space << std::endl; 00459 std::cerr << "Minimum plane value in HS for global space: " << settings.param_search_minimum_global_space << std::endl; 00460 std::cerr << "Hough space local maximum neighborhood search: " << settings.param_search_maxima_search_neighborhood << std::endl; 00461 std::cerr << "Hough space local maximum search blur: " << settings.param_search_maxima_search_blur << std::endl; 00462 std::cerr << std::endl; 00463 std::cerr << "Given visualisation parameters:" << std::endl; 00464 std::cerr << "==============================" << std::endl; 00465 std::cerr << "Maximum point distance from plane: " << settings.param_visualisation_distance << std::endl; 00466 std::cerr << "Maximum point normal deviation from plane: " << settings.param_visualisation_plane_normal_dev << std::endl; 00467 std::cerr << "Maximum point plane shift (d param) from plane: " << settings.param_visualisation_plane_shift_dev << std::endl; 00468 std::cerr << "Minimum point count for visualised plane in current point cloud: " << settings.param_visualisation_min_count << std::endl; 00469 std::cerr << std::endl; 00470 00471 ROS_INFO("Plane det. input: %s", settings.param_node_input.c_str()); 00472 ROS_INFO("Plane coloring: %s", settings.param_visualisation_color.c_str()); 00473 00474 // if PCL input 00475 if (settings.param_node_input == PARAM_NODE_INPUT_PCL) 00476 { 00477 exporter = new DynModelExporter(n, 00478 settings.param_original_frame, 00479 settings.param_output_frame, 00480 settings.param_visualisation_min_count, 00481 settings.param_visualisation_distance, 00482 settings.param_visualisation_plane_normal_dev, 00483 settings.param_visualisation_plane_shift_dev, 00484 settings.param_ht_keeptrack, 00485 settings.param_visualisation_ttl, 00486 settings.param_visualisation_max_poly_size); 00487 00488 message_filters::Subscriber<PointCloud2 > point_cloud(*n, DET_INPUT_POINT_CLOUD_TOPIC, 10); 00489 00490 pub1 = n->advertise<pcl::PointCloud<pcl::PointXYZRGB> > (DET_OUTPUT_POINT_CLOUD_TOPIC, 1); 00491 pub2 = n->advertise<visualization_msgs::MarkerArray > (DET_OUTPUT_MARKER_TOPIC, 1); 00492 pub3 = n->advertise<cob_3d_mapping_msgs::ShapeArray > (DET_OUTPUT_MARKER_SRS_TOPIC, 1); 00493 00494 ros::ServiceServer service1 = n->advertiseService(DET_SERVICE_RESET_PLANES, onReset); 00495 ros::ServiceServer service2 = n->advertiseService(DET_SERVICE_SAVE_PLANES, onSave); 00496 ros::ServiceServer service3 = n->advertiseService(DET_SERVICE_LOAD_PLANES, onLoad); 00497 00498 // MESSAGES 00499 if (settings.param_visualisation_color == "mean_color") 00500 { 00501 // sync images 00502 tfListener = new tf::TransformListener(); 00503 transform_filter = new tf::MessageFilter<sensor_msgs::PointCloud2> (point_cloud, *tfListener, settings.param_output_frame, 10); 00504 00505 message_filters::Subscriber<Image> sub_rgb(*n, DET_INPUT_RGB_IMAGE_TOPIC, 10); 00506 00507 typedef sync_policies::ApproximateTime<PointCloud2, Image> tSyncPolicy; 00508 Synchronizer<tSyncPolicy> sync(tSyncPolicy(10), *transform_filter, sub_rgb); 00509 00510 // register callback called when everything synchronized arrives 00511 sync.registerCallback(boost::bind(&callbackpcl_rgb, _1, _2)); 00512 00513 std::cerr << "Plane detector initialized and listening point clouds..." << std::endl; 00514 ros::spin(); 00515 00516 return 1; 00517 } 00518 else 00519 { 00520 // sync images 00521 tfListener = new tf::TransformListener(); 00522 transform_filter = new tf::MessageFilter<sensor_msgs::PointCloud2> (point_cloud, *tfListener, settings.param_output_frame, 10); 00523 transform_filter->registerCallback(boost::bind(&callbackpcl, _1)); 00524 00525 std::cerr << "Plane detector initialized and listening point clouds..." << std::endl; 00526 ros::spin(); 00527 00528 return 1; 00529 } 00530 } 00531 00532 // if kinect input 00533 else if (settings.param_node_input == PARAM_NODE_INPUT_KINECT) 00534 { 00535 exporter = new DynModelExporter(n, 00536 settings.param_original_frame, 00537 settings.param_output_frame, 00538 settings.param_visualisation_min_count, 00539 settings.param_visualisation_distance, 00540 settings.param_visualisation_plane_normal_dev, 00541 settings.param_visualisation_plane_shift_dev, 00542 settings.param_ht_keeptrack, 00543 settings.param_visualisation_ttl, 00544 settings.param_visualisation_max_poly_size); 00545 00546 // MESSAGES 00547 message_filters::Subscriber<Image> depth_sub(*n, DET_INPUT_IMAGE_TOPIC, 1); 00548 message_filters::Subscriber<CameraInfo> info_sub_depth(*n, DET_INPUT_CAM_INFO_TOPIC, 1); 00549 00550 pub1 = n->advertise<pcl::PointCloud<pcl::PointXYZRGB> > (DET_OUTPUT_POINT_CLOUD_TOPIC, 1); 00551 00552 00553 ros::ServiceServer service1 = n->advertiseService(DET_SERVICE_RESET_PLANES, onReset); 00554 ros::ServiceServer service2 = n->advertiseService(DET_SERVICE_SAVE_PLANES, onSave); 00555 ros::ServiceServer service3 = n->advertiseService(DET_SERVICE_LOAD_PLANES, onLoad); 00556 00557 00558 typedef sync_policies::ApproximateTime<Image, CameraInfo> MySyncPolicy; 00559 Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), depth_sub, info_sub_depth); 00560 sync.registerCallback(boost::bind(&callbackkinect, _1, _2)); 00561 std::cerr << "Plane detector initialized and listening depth images..." << std::endl; 00562 ros::spin(); 00563 00564 return 1; 00565 } 00566 }