Attachment 'pcl_cylinder.cpp'
Download 1 #include <ros/ros.h>
2 #include <sensor_msgs/PointCloud2.h>
3
4 #include <pcl/ros/conversions.h>
5 #include <pcl/point_types.h>
6 #include <pcl_ros/transforms.h>
7 #include <pcl/point_cloud.h>
8 #include <pcl/point_types.h>
9 #include <pcl_ros/point_cloud.h>
10
11 #include <pcl/io/pcd_io.h>
12 #include <pcl/ModelCoefficients.h>
13
14 #include <pcl/filters/passthrough.h>
15 #include <pcl/filters/extract_indices.h>
16
17 #include <pcl/features/normal_3d.h>
18
19 #include <pcl/sample_consensus/method_types.h>
20 #include <pcl/sample_consensus/model_types.h>
21 #include <pcl/segmentation/sac_segmentation.h>
22
23 #include "tf/transform_broadcaster.h"
24 #include "tf/transform_listener.h"
25 #include "std_srvs/Empty.h"
26
27 ros::Publisher plane_pub;
28 ros::Publisher cylinder_pub;
29 ros::Publisher filter_pub;
30 int i = 0;
31 bool detect = false;
32 bool cylinder_found = false;
33
34 static double table_height = 0.72;
35 static tf::TransformBroadcaster* pBroadcaster = NULL;
36 static tf::TransformListener* pListener = NULL;
37 static tf::StampedTransform odom_camera_transform;
38 static tf::Transform cylinder_transform;
39 static sensor_msgs::PointCloud2::Ptr buffer (new sensor_msgs::PointCloud2);
40
41 bool startCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
42 {
43 ROS_INFO("start detecting");
44 detect = true;
45 cylinder_found = false;
46 return true;
47 }
48
49 bool stopCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
50 {
51 ROS_INFO("stop detecting");
52 detect = false;
53 cylinder_found = false;
54 return true;
55 }
56
57 void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
58 {
59 if(cylinder_found){
60 pBroadcaster->sendTransform(tf::StampedTransform(cylinder_transform, ros::Time::now(), "/odom", "/cylinder_center"));
61 }
62
63 if(i < 8 ){ //rate limiting
64 i++;
65 ros::spinOnce();
66 } else if (!detect){
67 i = 0;
68 } else {
69 i = 0;
70
71 // filter
72 pcl::PassThrough<sensor_msgs::PointCloud2> pass;
73 pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
74 pcl::ExtractIndices<pcl::Normal> extract_normals;
75
76 // Normal estimation
77 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
78 pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;
79
80 // Create the segmentation object
81 pcl::SACSegmentation<pcl::PointXYZ> seg;
82
83 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
84 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
85 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
86
87 // The plane and cylinder coefficients
88 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
89 pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
90 pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
91
92 // The plane and cylinder inliers
93 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
94 pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
95 pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
96
97 // The point clouds
98 sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
99 sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
100 sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
101 sensor_msgs::PointCloud2::Ptr cloud_transformed (new sensor_msgs::PointCloud2);
102
103 // The PointCloud
104 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
105 pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
106 pcl::PointCloud<pcl::PointXYZ>::Ptr plane_output (new pcl::PointCloud<pcl::PointXYZ>);
107 pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
108 pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
109 pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
110
111 // The cloud normals
112 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane
113 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder
114 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ());
115
116
117 // Convert PointCloud2 to /odom frame
118 try{
119 pListener->waitForTransform("/odom", (*cloud).header.frame_id, (*cloud).header.stamp, ros::Duration(10.0) );
120 pcl_ros::transformPointCloud ("/odom", *cloud, *cloud_transformed, *pListener);
121 }
122 catch(tf::TransformException e){
123 ROS_INFO("Transform_PointCloud: %s", e.what());
124 return;
125 }
126
127 //Pass through Filtering
128 pass.setInputCloud (cloud_transformed);
129 pass.setFilterFieldName ("x");
130 pass.setFilterLimits (0.35, 0.55);
131 pass.filter (*passthrough_filtered);
132
133 pass.setInputCloud (passthrough_filtered);
134 pass.setFilterFieldName ("y");
135 pass.setFilterLimits (-0.08, 0.22);
136 pass.filter (*passthrough_filtered);
137
138 pass.setInputCloud (passthrough_filtered);
139 pass.setFilterFieldName ("z");
140 pass.setFilterLimits (0.70, 0.85);
141 pass.filter (*passthrough_filtered);
142
143 //publish the filtered cloud
144 filter_pub.publish(passthrough_filtered);
145
146 // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
147 pcl::fromROSMsg (*passthrough_filtered, *plane_cloud);
148
149
150 // Estimate point normals
151 normal_estimation.setSearchMethod (tree2);
152 normal_estimation.setInputCloud (plane_cloud);
153 normal_estimation.setKSearch (25);
154 normal_estimation.compute (*cloud_normals2);
155
156 // Create the segmentation object for the planar model and set all the parameters
157 segmentation_from_normals.setOptimizeCoefficients (true);
158 segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
159 segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
160 segmentation_from_normals.setNormalDistanceWeight (0.1);
161 segmentation_from_normals.setMaxIterations (5000);
162 segmentation_from_normals.setDistanceThreshold (0.06);
163 segmentation_from_normals.setInputCloud (plane_cloud);
164 segmentation_from_normals.setInputNormals (cloud_normals2);
165
166 // Obtain the plane inliers and coefficients
167 segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
168
169 // Extract the planar inliers from the input cloud
170 extract_indices.setInputCloud (plane_cloud);
171 extract_indices.setIndices (inliers_plane);
172 extract_indices.setNegative (true);
173 extract_indices.filter (*cylinder_cloud);
174 extract_indices.setInputCloud (plane_cloud);
175 extract_indices.setIndices (inliers_plane);
176 extract_indices.setNegative (false);
177 extract_indices.filter (*plane_output);
178 extract_normals.setNegative (true);
179 extract_normals.setInputCloud (cloud_normals2);
180 extract_normals.setIndices (inliers_plane);
181 extract_normals.filter (*cloud_normals3);
182
183 //colorize plane cloud and publish it
184 pcl::PointCloud<pcl::PointXYZRGB> plane_color;
185 plane_color.width = plane_output->width;
186 plane_color.height = plane_output->height;
187 plane_color.is_dense = true;
188 plane_color.points.resize (plane_color.width * plane_color.height);
189 plane_color.sensor_origin_ = plane_output->sensor_origin_;
190 plane_color.sensor_orientation_ = plane_output->sensor_orientation_;
191 plane_color.header = plane_output->header;
192
193 for(int i = 0; i < plane_color.size(); i++){
194 plane_color.points[i].x = plane_output->points[i].x;
195 plane_color.points[i].y = plane_output->points[i].y;
196 plane_color.points[i].z = plane_output->points[i].z;
197 plane_color.points[i].r = 0;
198 plane_color.points[i].g = 240;
199 plane_color.points[i].b = 0;
200 }
201
202 pcl::toROSMsg (plane_color, *plane_output_cloud);
203 plane_pub.publish(plane_output_cloud);
204
205 // Create the segmentation object for cylinder segmentation and set all the openniparameters
206 segmentation_from_normals.setOptimizeCoefficients (true);
207 segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
208 segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
209 segmentation_from_normals.setNormalDistanceWeight (0.1);
210 segmentation_from_normals.setMaxIterations (5000);
211 segmentation_from_normals.setDistanceThreshold (0.02);
212 segmentation_from_normals.setRadiusLimits (0.02, 0.05);
213 segmentation_from_normals.setInputCloud (cylinder_cloud);
214 segmentation_from_normals.setInputNormals (cloud_normals3);
215
216 // Obtain the cylinder inliers and coefficients
217 segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
218
219 // Publish the cylinder cloud
220 extract_indices.setInputCloud (cylinder_cloud);
221 extract_indices.setIndices (inliers_cylinder);
222 extract_indices.setNegative (false);
223 extract_indices.filter (*cylinder_output);
224
225 if (cylinder_output->points.empty ()){
226 std::cerr << "Can't find the cylinder component." << std::endl;
227 return;
228 }
229
230 //colorize the clyinder cloud and publish it
231 pcl::PointCloud<pcl::PointXYZRGB> cylinder_color;
232 cylinder_color.width = cylinder_output->width;
233 cylinder_color.height = cylinder_output->height;
234 cylinder_color.is_dense = true;
235 cylinder_color.points.resize (cylinder_color.width * cylinder_color.height);
236 cylinder_color.sensor_origin_ = cylinder_output->sensor_origin_;
237 cylinder_color.sensor_orientation_ = cylinder_output->sensor_orientation_;
238 cylinder_color.header = cylinder_output->header;
239
240 for(int i = 0; i < cylinder_color.size(); i++){
241 cylinder_color.points[i].x = cylinder_output->points[i].x;
242 cylinder_color.points[i].y = cylinder_output->points[i].y;
243 cylinder_color.points[i].z = cylinder_output->points[i].z;
244 cylinder_color.points[i].r = 238;
245 cylinder_color.points[i].g = 0;
246 cylinder_color.points[i].b = 0;
247 }
248 pcl::toROSMsg (cylinder_color, *cylinder_output_cloud);
249 cylinder_pub.publish(cylinder_output_cloud);
250
251 //create a transform for the center of the cylinder
252 tf::Transform transform;
253 tf::Vector3 origin;
254 origin.setX(coefficients_cylinder->values[0]);
255 origin.setY(coefficients_cylinder->values[1]);
256 origin.setZ(coefficients_cylinder->values[2]);
257 transform.setOrigin(origin);
258 transform.setRotation(tf::Quaternion(0,0,0));
259
260 double x_axis = coefficients_cylinder->values[3];
261 double y_axis = coefficients_cylinder->values[4];
262 double z_axis = coefficients_cylinder->values[5];
263
264 ROS_INFO("x = %f", x_axis);
265 ROS_INFO("y = %f", y_axis);
266 ROS_INFO("z = %f", z_axis);
267
268 //does the coordinate frame match our criteria
269 bool publish = coefficients_cylinder->values[6] > 0;
270 publish = publish && fabs(x_axis) < 0.2;
271 publish = publish && fabs(y_axis) < 0.2;
272 publish = publish && fabs(z_axis) > 0.8;
273 publish = publish && fabs(sqrt(x_axis*x_axis+y_axis*y_axis+z_axis*z_axis)) < 1.2;
274 publish = publish && transform.getOrigin().getX() > 0.35;
275 publish = publish && transform.getOrigin().getX() < 0.60;
276 publish = publish && transform.getOrigin().getY() > -0.08;
277 publish = publish && transform.getOrigin().getY() < 0.22;
278 publish = publish && transform.getOrigin().getZ() > table_height;
279 publish = publish && transform.getOrigin().getZ() < table_height+0.1;
280
281 //publish the center of the cylinder as tf_frame
282 if(publish){
283 ROS_INFO("New cylinder frame published");
284 cylinder_found = true;
285 tf::Vector3 transform_origin = transform.getOrigin();
286 transform_origin.setZ(table_height + 0.05);
287 transform.setOrigin(transform_origin);
288 cylinder_transform = transform;
289 }
290 }
291 }
292
293 int
294 main (int argc, char** argv)
295 {
296 //INITIALIZE ROS
297 ros::init (argc, argv, "extract_cylinder");
298 ros::NodeHandle nh;
299 pListener = new(tf::TransformListener);
300 pBroadcaster = new(tf::TransformBroadcaster);
301 ros::ServiceServer start_srv = nh.advertiseService("start_detecting_srv", startCallback);
302 ros::ServiceServer stop_srv = nh.advertiseService("stop_detecting_srv", stopCallback);
303
304 ros::Subscriber sub = nh.subscribe("cloud_throttled", 1, callback);
305 filter_pub = nh.advertise<sensor_msgs::PointCloud2> ("filter_cloud", 1);
306 plane_pub = nh.advertise<sensor_msgs::PointCloud2> ("plane_cloud", 1);
307 cylinder_pub = nh.advertise<sensor_msgs::PointCloud2> ("cylinder_cloud", 1);
308
309 ros::spin();
310
311 return (0);
312 }
Attached Files
To refer to attachments on a page, use attachment:filename, as shown below in the list of files. Do NOT use the URL of the [get] link, since this is subject to change and can break easily.You are not allowed to attach a file to this page.
