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
00026
00027
00037 #include <ros/ros.h>
00038 #include <image_transport/image_transport.h>
00039 #include <cv_bridge/cv_bridge.h>
00040
00041
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
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
00145 pcl::PointCloud<pcl::PointXYZ> pointcloud;
00146 pcl::fromROSMsg (*cloud, pointcloud);
00147
00148
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
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
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
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
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
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
00206 seg.setOptimizeCoefficients (true);
00207
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
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
00246
00247
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
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
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
00437 ros::NodeHandle private_nh("~");
00438 getParams(private_nh);
00439
00440
00441
00442
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
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
00499 if (settings.param_visualisation_color == "mean_color")
00500 {
00501
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
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
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
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
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 }