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 #define PFLN printf("file %s line %d\n",__FILE__,__LINE__);
51 
52 #define _USE_DEBUG_ 0
53 //Global variables
54 ros::NodeHandle* p_n;
55 tf::TransformListener *p_listener;
56 bool process_laser_scan[5]={false,false,false,false,false};
57 bool process_point_cloud[5]={false,false,false,false,false};
58 pcl::PointCloud<pcl::PointXYZ> pc_accumulated;
59 pcl::PointCloud<pcl::PointXYZ> convex_hull;
61 double output_freq;
62 
63 void filter_cloud(pcl::PointCloud<pcl::PointXYZ>* pc_in, std::string pc_frame_id)
64 {
65  pcl::PointCloud<pcl::PointXYZ> pc_transformed;
66  pcl::PointCloud<pcl::PointXYZ> pc_filtered;
67  pcl::PointCloud<pcl::PointXYZ> pc_projected;
68  tf::StampedTransform transform;
69 
70  //p_listener->waitForTransform(pcmsg_in.header.frame_id, ros::names::remap("/tracking_frame"),ros::Time::now(), ros::Duration(0.2));
71  try
72  {
73  p_listener->lookupTransform(pc_frame_id, ros::names::remap("/tracking_frame"), ros::Time(0), transform);
74  //p_listener->lookupTransform(pcmsg_in.header.frame_id, ros::names::remap("/tracking_frame"), scan_in->header.stamp, transform);
75  }
76  catch (tf::TransformException ex)
77  {
78  ROS_ERROR("%s",ex.what());
79  }
80 
81 
82  pcl_ros::transformPointCloud(*pc_in,pc_transformed,transform.inverse());
83  pc_transformed.header.frame_id = ros::names::remap("/tracking_frame");
84  //pc_transformed.header.stamp = scan_in->header.stamp;
85 
86  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> epp;
87  pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud_constptr;
88  input_cloud_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (pc_transformed));
89  pcl::PointCloud<pcl::PointXYZ>::ConstPtr convex_hull_constptr;
90  convex_hull_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (convex_hull));
91 
92  //pcl::PointIndices::Ptr ind = pcl::PointIndices::Ptr(new pcl::PointIndices);
93  pcl::ExtractIndices<pcl::PointXYZ> extract; //Create the extraction object
94  pcl::PointIndices::Ptr indices;
95  indices.reset();
96  indices = pcl::PointIndices::Ptr(new pcl::PointIndices);
97 
98  //Set epp parameters
99  epp.setInputCloud(input_cloud_constptr);
100  epp.setInputPlanarHull(convex_hull_constptr);
101  epp.setHeightLimits(-perpendicular_treshold, perpendicular_treshold);
102  epp.setViewPoint(0,0,0); //i dont think this serves any purpose in the case of epp
103  epp.segment(*indices);
104 
105  extract.setInputCloud(pc_transformed.makeShared());
106  extract.setIndices(indices);
107  extract.setNegative(false);
108  extract.filter(pc_filtered);
109 
110  pcl::ProjectInliers<pcl::PointXYZ> projection;
111  projection.setModelType(pcl::SACMODEL_PLANE); //set model type
112  pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
113  coeff->values.resize(4);
114  coeff->values[0] = 0;
115  coeff->values[1] = 0;
116  coeff->values[2] = 1;
117  coeff->values[3] = 0;
118 
119  projection.setInputCloud(pc_filtered.makeShared());
120  projection.setModelCoefficients(coeff);
121  projection.filter(pc_projected);
122 
123  coeff.reset();
124  input_cloud_constptr.reset();
125  convex_hull_constptr.reset();
126  indices.reset();
127 
128  pc_accumulated += pc_projected;
129 
130 #if _USE_DEBUG_
131  ROS_INFO("pc_transformed has %d points", (int)pc_transformed.size());
132  ROS_INFO("pc_filtered has %d points", (int)pc_filtered.size());
133  ROS_INFO("pc_accumulated now has %d points", (int)pc_accumulated.size());
134 #endif
135 }
136 
137 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr & pcmsg_in)
138 {
139 
140 #if _USE_DEBUG_
141  ROS_INFO("Received point cloud");
142 #endif
143  pcl::PointCloud<pcl::PointXYZ> pc_in;
144  pcl::PCLPointCloud2 pcl_pc;
145  pcl_conversions::toPCL(*pcmsg_in, pcl_pc);
146 
147  pcl::fromROSMsg(*pcmsg_in,pc_in);
148  filter_cloud(&pc_in, pcmsg_in->header.frame_id);
149 }
150 
151 void scan_cb (const sensor_msgs::LaserScan::ConstPtr& scan_in)
152 {
153 #if _USE_DEBUG_
154  ROS_INFO("Received laser scan");
155 #endif
156 
157  ROS_INFO("asdasd");
158  laser_geometry::LaserProjection projector;
159  sensor_msgs::PointCloud2 pcmsg_in;
160 
161  projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,*p_listener); // não é necessário
162  pcmsg_in.header.stamp=ros::Time();
163  pcmsg_in.header.frame_id=scan_in->header.frame_id;
164 
165  pcl::PointCloud<pcl::PointXYZ> pc_in;
166  pcl::fromROSMsg(pcmsg_in,pc_in);
167  filter_cloud(&pc_in, scan_in->header.frame_id);
168 }
169 
170 
171 
172 int main(int argc, char **argv)
173 {
174 
175  ros::init(argc, argv, "planar_pc_generator_node");
176  ros::NodeHandle n("~");
177  tf::TransformListener listener(n,ros::Duration(10));
178  p_listener=&listener;
179  p_n=&n;
180  n.param("output_frequency", output_freq, 30.0);
181  n.param("perpendicular_treshold", perpendicular_treshold, 0.1);
182 
183  //Test the remapping of /tracking_frame
184  if (ros::names::remap("/tracking_frame")=="/tracking_frame")
185  {
186  ROS_ERROR("/tracking_frame was not remapped. Aborting program.");
187  ros::shutdown();
188  }
189  pc_accumulated.header.frame_id = ros::names::remap("/tracking_frame");
190 
191  //Test the remapping of the /laserscan s
192  for (int i=0; i<5; i++)
193  {
194  char str[1024];
195  sprintf(str,"/laserscan%d", i);
196  if (ros::names::remap(str)!=str)
197  {
198  process_laser_scan[i] = true;
199  }
200  }
201 
202  //Test the remapping of the /pointcloud s
203  for (int i=0; i<5; i++)
204  {
205  char str[1024];
206  sprintf(str,"/pointcloud%d", i);
207  if (ros::names::remap(str)!=str)
208  {
209  process_point_cloud[i] = true;
210  }
211  }
212 
213  std::vector<ros::Subscriber> sub;
214 
215  //Creates ROS subscribers if the laserscan topics where remapped
216  for (int i=0; i<5; i++)
217  {
218  if (process_laser_scan[i]==true)
219  {
220  char str[1024];
221  sprintf(str,"/laserscan%d", i);
222  ros::Subscriber sub_ = n.subscribe (str, 1, scan_cb);
223  ROS_INFO("Subscribing to %s", (ros::names::remap(str)).c_str());
224  sub.push_back(sub_);
225  }
226  }
227 
228  //Creates ROS subscribers if the pointcloud topics where remapped
229  for (int i=0; i<5; i++)
230  {
231  if (process_point_cloud[i]==true)
232  {
233  char str[1024];
234  sprintf(str,"/pointcloud%d", i);
235  ros::Subscriber sub_ = n.subscribe (str, 1, cloud_cb);
236  ROS_INFO("Subscribing to %s", (ros::names::remap(str)).c_str());
237  sub.push_back(sub_);
238  }
239  }
240 
241  if (sub.size()==0)
242  {
243  ROS_ERROR("No /laserscan[0 4] or /pointcloud[0 4] where remapped. Aborting...");
244  ros::shutdown();
245  }
246 
247  //declare the publisher
248  ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>("/pc_out", 1);
249 
250  //compute the convex hull to use in extract polygonal prism data
251  //Its just a very large square (500m*2 of side) around the /tracking_frame origin
252  convex_hull.header.frame_id = ros::names::remap("/tracking_frame");
253  pcl::PointXYZ pt;
254  pt.x = -500; pt.y=-500; pt.z=0;
255  convex_hull.points.push_back(pt);
256 
257  pt.x = 500; pt.y=-500; pt.z=0;
258  convex_hull.points.push_back(pt);
259 
260  pt.x = 500; pt.y= 500; pt.z=0;
261  convex_hull.points.push_back(pt);
262 
263  pt.x = -500; pt.y=500; pt.z=0;
264  convex_hull.points.push_back(pt);
265 
266  //ros::Snooze snooze(Duration(0,20000000); // 20ms snooze
267  //snooze.initialise();
268 
269  //cloud_pub.publish(pcmsg_out);
270 
271  ros::Rate loop_rate(output_freq);
272  while (n.ok())
273  {
274 
275  //ros::spinOnce();
276  //ros::Duration(0.05).sleep();
277 
278  ros::spinOnce();
279 
280  if (pc_accumulated.points.size()!=0)
281  {
282  sensor_msgs::PointCloud2 pcmsg_out;
283  pcl::toROSMsg(pc_accumulated, pcmsg_out);
284  pcmsg_out.header.stamp = ros::Time::now();
285 #if _USE_DEBUG_
286  ROS_INFO("Publishing pc_accumulated with %d points", (int)pc_accumulated.size());
287 #endif
288  pub.publish(pcmsg_out);
289  pc_accumulated.erase(pc_accumulated.begin(), pc_accumulated.end());
290  }
291 
292  loop_rate.sleep();
293  }
294 }
bool process_point_cloud[5]
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &pcmsg_in)
tf::TransformListener * p_listener
void filter_cloud(pcl::PointCloud< pcl::PointXYZ > *pc_in, std::string pc_frame_id)
ros::NodeHandle * p_n
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
bool process_laser_scan[5]
double output_freq
void scan_cb(const sensor_msgs::LaserScan::ConstPtr &scan_in)
double perpendicular_treshold
int main(int argc, char **argv)
pcl::PointCloud< pcl::PointXYZ > convex_hull


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