simple_planar_pc_generator.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
35 #include <stdio.h>
36 #include <ros/ros.h>
37 #include <laser_geometry/laser_geometry.h>
38 #include <sensor_msgs/LaserScan.h>
39 #include <sensor_msgs/PointCloud2.h>
40 #include <tf/transform_listener.h>
41 #include <pcl_ros/transforms.h>
42 #include <pcl/conversions.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 #include <pcl/segmentation/extract_polygonal_prism_data.h>
46 #include <pcl/filters/extract_indices.h>
47 #include <pcl/filters/project_inliers.h>
48 #include <pcl_conversions/pcl_conversions.h>
49 
50 class PointCloudGeneretor
51 {
52  public:
53  ros::NodeHandle nh;
54  ros::Publisher points_publisher;
55  tf::TransformListener transform_listener;
56 
57  bool wait_for_laser_1;
58  bool wait_for_laser_2;
59 
60  bool laserscan_1_arrived;
61  bool laserscan_2_arrived;
62 
63  double output_freq;
64  double perpendicular_treshold;
65 
66  pcl::PointCloud<pcl::PointXYZ> pc_accumulated;
67  pcl::PointCloud<pcl::PointXYZ> convex_hull;
68 
69  ros::Subscriber scan_1_subscriber;
70  ros::Subscriber scan_2_subscriber;
71 
72  PointCloudGeneretor(ros::NodeHandle nh_)
73  :nh(nh_),
74  transform_listener(nh_,ros::Duration(10))
75  {
76  laserscan_1_arrived = false;
77  laserscan_2_arrived = false;
78 
79  nh.param("wait_for_laser_1", wait_for_laser_1, true);
80  nh.param("wait_for_laser_2", wait_for_laser_2, true);
81 
82  nh.param("output_frequency", output_freq, 200.0);
83  nh.param("perpendicular_treshold", perpendicular_treshold, 0.2);
84 
85  //Test the remapping of /tracking_frame
86  if (ros::names::remap("/tracking_frame")=="/tracking_frame")
87  {
88  ROS_ERROR("/tracking_frame was not remapped. Aborting program.");
89  ros::shutdown();
90  }
91 
92  pc_accumulated.header.frame_id = ros::names::remap("/tracking_frame");
93 
94  scan_1_subscriber = nh.subscribe ("/laserscan0", 1000, &PointCloudGeneretor::scan1Handler,this);
95  scan_2_subscriber = nh.subscribe ("/laserscan1", 1000, &PointCloudGeneretor::scan2Handler,this);
96 
97  //declare the publisher
98  points_publisher = nh.advertise<sensor_msgs::PointCloud2>("/pc_out", 1000);
99 
100 
101  //compute the convex hull to use in extract polygonal prism data
102  //Its just a very large square (500m*2 of side) around the /tracking_frame origin
103  convex_hull.header.frame_id = ros::names::remap("/tracking_frame");
104  pcl::PointXYZ pt;
105  pt.x = -500; pt.y=-500; pt.z=0;
106  convex_hull.points.push_back(pt);
107 
108  pt.x = 500; pt.y=-500; pt.z=0;
109  convex_hull.points.push_back(pt);
110 
111  pt.x = 500; pt.y= 500; pt.z=0;
112  convex_hull.points.push_back(pt);
113 
114  pt.x = -500; pt.y=500; pt.z=0;
115  convex_hull.points.push_back(pt);
116  }
117 
118  void filterCloud(pcl::PointCloud<pcl::PointXYZ>* pc_in, std::string pc_frame_id)
119  {
120  pcl::PointCloud<pcl::PointXYZ> pc_transformed;
121  pcl::PointCloud<pcl::PointXYZ> pc_filtered;
122  pcl::PointCloud<pcl::PointXYZ> pc_projected;
123  tf::StampedTransform transform;
124 
125  try
126  {
127  transform_listener.lookupTransform(pc_frame_id, ros::names::remap("/tracking_frame"), ros::Time(0), transform);
128  //p_listener->lookupTransform(pcmsg_in.header.frame_id, ros::names::remap("/tracking_frame"), scan_in->header.stamp, transform);
129  }
130  catch (tf::TransformException ex)
131  {
132  ROS_ERROR("%s",ex.what());
133  }
134 
135 
136  pcl_ros::transformPointCloud(*pc_in,pc_transformed,transform.inverse());
137  pc_transformed.header.frame_id = ros::names::remap("/tracking_frame");
138  //pc_transformed.header.stamp = scan_in->header.stamp;
139 
140  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> epp;
141  pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud_constptr;
142  input_cloud_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (pc_transformed));
143  pcl::PointCloud<pcl::PointXYZ>::ConstPtr convex_hull_constptr;
144  convex_hull_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (convex_hull));
145 
146  //pcl::PointIndices::Ptr ind = pcl::PointIndices::Ptr(new pcl::PointIndices);
147  pcl::ExtractIndices<pcl::PointXYZ> extract; //Create the extraction object
148  pcl::PointIndices::Ptr indices;
149  indices.reset();
150  indices = pcl::PointIndices::Ptr(new pcl::PointIndices);
151 
152  //Set epp parameters
153  epp.setInputCloud(input_cloud_constptr);
154  epp.setInputPlanarHull(convex_hull_constptr);
155  epp.setHeightLimits(-perpendicular_treshold, perpendicular_treshold);
156  epp.setViewPoint(0,0,0); //i dont think this serves any purpose in the case of epp
157  epp.segment(*indices);
158 
159  extract.setInputCloud(pc_transformed.makeShared());
160  extract.setIndices(indices);
161  extract.setNegative(false);
162  extract.filter(pc_filtered);
163 
164  pcl::ProjectInliers<pcl::PointXYZ> projection;
165  projection.setModelType(pcl::SACMODEL_PLANE); //set model type
166  pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
167  coeff->values.resize(4);
168  coeff->values[0] = 0;
169  coeff->values[1] = 0;
170  coeff->values[2] = 1;
171  coeff->values[3] = 0;
172 
173  projection.setInputCloud(pc_filtered.makeShared());
174  projection.setModelCoefficients(coeff);
175  projection.filter(pc_projected);
176 
177  coeff.reset();
178  input_cloud_constptr.reset();
179  convex_hull_constptr.reset();
180  indices.reset();
181 
182  pc_accumulated += pc_projected;
183 
184  #if _USE_DEBUG_
185  ROS_INFO("pc_transformed has %d points", (int)pc_transformed.size());
186  ROS_INFO("pc_filtered has %d points", (int)pc_filtered.size());
187  ROS_INFO("pc_accumulated now has %d points", (int)pc_accumulated.size());
188  #endif
189  }
190 
191  void scan1Handler(const sensor_msgs::LaserScan::ConstPtr& scan_in)
192  {
193  laser_geometry::LaserProjection projector;
194  sensor_msgs::PointCloud2 pcmsg_in;
195 
196  projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,transform_listener); // não é necessário
197  pcmsg_in.header.stamp=ros::Time();
198  pcmsg_in.header.frame_id=scan_in->header.frame_id;
199 
200  pcl::PointCloud<pcl::PointXYZ> pc_in;
201  pcl::PCLPointCloud2 pcl_pc;
202  pcl_conversions::toPCL(pcmsg_in, pcl_pc);
203  pcl::fromPCLPointCloud2(pcl_pc, pc_in);
204 
205  filterCloud(&pc_in, scan_in->header.frame_id);
206 
207  laserscan_1_arrived=true;
208 
209  publishCloud();
210  }
211 
212  void scan2Handler(const sensor_msgs::LaserScan::ConstPtr& scan_in)
213  {
214  laser_geometry::LaserProjection projector;
215  sensor_msgs::PointCloud2 pcmsg_in;
216 
217  projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,transform_listener); // não é necessário
218  pcmsg_in.header.stamp=ros::Time();
219  pcmsg_in.header.frame_id=scan_in->header.frame_id;
220 
221  pcl::PointCloud<pcl::PointXYZ> pc_in;
222  pcl::PCLPointCloud2 pcl_pc;
223  pcl_conversions::toPCL(pcmsg_in, pcl_pc);
224  pcl::fromPCLPointCloud2(pcl_pc, pc_in);
225 
226  filterCloud(&pc_in, scan_in->header.frame_id);
227 
228  laserscan_2_arrived=true;
229 
230  publishCloud();
231  }
232 
233  void publishCloud()
234  {
235  if(!laserscan_1_arrived && wait_for_laser_1)
236  return;
237 
238  if(!laserscan_2_arrived && wait_for_laser_2)
239  return;
240 
241  laserscan_1_arrived=false;
242  laserscan_2_arrived=false;
243 
244  sensor_msgs::PointCloud2 pcmsg_out;
245  pcl::toROSMsg(pc_accumulated, pcmsg_out);
246  pcmsg_out.header.stamp = ros::Time::now();
247 
248  #if _USE_DEBUG_
249  ROS_INFO("Publishing pc_accumulated with %d points", (int)pc_accumulated.size());
250  #endif
251 
252  points_publisher.publish(pcmsg_out);
253  pc_accumulated.erase(pc_accumulated.begin(), pc_accumulated.end());
254  }
255 
256 };
257 
258 
259 
260 int main(int argc, char **argv)
261 {
262  ros::init(argc, argv, "simple_planar_pc_generator_node");
263  ros::NodeHandle n("~");
264 
265  PointCloudGeneretor generator(n);
266 
267  ros::spin();
268 }
PointCloudGeneretor::scan1Handler
void scan1Handler(const sensor_msgs::LaserScan::ConstPtr &scan_in)
Definition: simple_planar_pc_generator.cpp:191
PointCloudGeneretor::filterCloud
void filterCloud(pcl::PointCloud< pcl::PointXYZ > *pc_in, std::string pc_frame_id)
Definition: simple_planar_pc_generator.cpp:118
PointCloudGeneretor::convex_hull
pcl::PointCloud< pcl::PointXYZ > convex_hull
Definition: simple_planar_pc_generator.cpp:67
main
int main(int argc, char **argv)
Definition: simple_planar_pc_generator.cpp:260
PointCloudGeneretor::transform_listener
tf::TransformListener transform_listener
Definition: simple_planar_pc_generator.cpp:55
PointCloudGeneretor::pc_accumulated
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
Definition: simple_planar_pc_generator.cpp:66
PointCloudGeneretor::PointCloudGeneretor
PointCloudGeneretor(ros::NodeHandle nh_)
Definition: simple_planar_pc_generator.cpp:72
PointCloudGeneretor::scan2Handler
void scan2Handler(const sensor_msgs::LaserScan::ConstPtr &scan_in)
Definition: simple_planar_pc_generator.cpp:212

mtt
Author(s): Jorge Almeida
autogenerated on Mon Mar 2 2015 01:32:18