1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38 #include <ros/ros.h>
39 #include <pcl/PCLPointCloud2.h>
40 #include <message_filters/subscriber.h>
41 #include <message_filters/synchronizer.h>
42 #include <message_filters/sync_policies/approximate_time.h>
43 #include <pcl_cuda/io/disparity_to_cloud.h>
44 #include <pcl_cuda/sample_consensus/sac_model_plane.h>
45 #include <pcl_cuda/sample_consensus/ransac.h>
46
47 #include <pcl/point_cloud.h>
48 #include <pcl_ros/point_cloud.h>
49 #include <pcl/point_types.h>
50
51 #include <functional>
52
53 using namespace message_filters;
54 using namespace pcl;
55 using namespace pcl_cuda;
56
57 DisparityToCloud d2c;
58 ros::Publisher pub;
59
60 struct EventHelper
61 {
62 void
callbackEventHelper63 callback (const pcl::PCLImage::ConstPtr &depth,
64 const pcl::PCLImage::ConstPtr &rgb,
65 const pcl::CameraInfo::ConstPtr &info)
66 {
67 //using Indices = pcl_cuda::SampleConsensusModel<pcl_cuda::Host>::Indices;
68
69 //pcl_cuda::PointCloudAOS<pcl_cuda::Host>::Ptr data (new pcl_cuda::PointCloudAOS<pcl_cuda::Host>);
70 PointCloudAOS<Device>::Ptr data;
71 d2c.callback (depth, rgb, info, data);
72
73 SampleConsensusModelPlane<Device>::Ptr sac_model (new SampleConsensusModelPlane<Device> (data));
74 // SampleConsensusModelPlane<Host>::Ptr sac_model (new pcl_cuda::SampleConsensusModelPlane<pcl_cuda::Host> (data));
75 RandomSampleConsensus<Device> sac (sac_model);
76 sac.setDistanceThreshold (0.05);
77
78 if (!sac.computeModel (2))
79 std::cerr << "Failed to compute model" << std::endl;
80
81 // Convert data from Thrust/HOST to PCL
82 pcl::PointCloud<pcl::PointXYZRGB> output;
83 // pcl_cuda::toPCL (*data, output);
84
85 output.header.stamp = ros::Time::now ();
86 pub.publish (output);
87 }
88 };
89
90 int
main(int argc,char ** argv)91 main (int argc, char **argv)
92 {
93 ros::init (argc, argv, "disparity_to_cloud");
94 ros::NodeHandle nh;
95
96 // Prepare output
97 pub = nh.advertise<PCLPointCloud2>("output", 1);
98
99 // Subscribe to topics
100 Synchronizer<sync_policies::ApproximateTime<PCLImage, PCLImage, CameraInfo> > sync_rgb (30);
101 Synchronizer<sync_policies::ApproximateTime<PCLImage, CameraInfo> > sync (30);
102 Subscriber<PCLImage> sub_depth, sub_rgb;
103 Subscriber<CameraInfo> sub_info;
104 sub_depth.subscribe (nh, "/camera/depth/image", 30);
105 sub_rgb.subscribe (nh, "/camera/rgb/image_color", 30);
106 sub_info.subscribe (nh, "/camera/rgb/camera_info", 120);
107 //sub_info.subscribe (nh, "/camera/depth/camera_info", 120);
108
109 EventHelper h;
110
111 if (argc > 1 && atoi (argv[1]) == 1)
112 {
113 ROS_INFO ("[disparity_to_cloud] Using RGB to color the points.");
114 sync_rgb.connectInput (sub_depth, sub_rgb, sub_info);
115 //sync_rgb.registerCallback (bind (&pcl_cuda::DisparityToCloud::callback, k, _1, _2, _3));
116 sync_rgb.registerCallback (std::bind (&EventHelper::callback, &h, _1, _2, _3));
117 }
118 else
119 {
120 sync.connectInput (sub_depth, sub_info);
121 //sync.registerCallback (bind (&pcl_cuda::DisparityToCloud::callback, k, _1, PCLImageConstPtr (), _2));
122 sync.registerCallback (std::bind (&EventHelper::callback, &h, _1, PCLImageConstPtr (), _2));
123 }
124
125 // Do this indefinitely
126 ros::spin ();
127
128 return (0);
129 }
130