simple_planar_pc_generator_atlasmv.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 ros::NodeHandle* p_n;
51 tf::TransformListener *p_listener;
52 pcl::PointCloud<pcl::PointXYZ> pc_accumulated;
53 pcl::PointCloud<pcl::PointXYZ> convex_hull;
55 double output_freq;
56 
58 
59 void filter_cloud(pcl::PointCloud<pcl::PointXYZ>* pc_in, std::string pc_frame_id)
60 {
61  pcl::PointCloud<pcl::PointXYZ> pc_transformed;
62  pcl::PointCloud<pcl::PointXYZ> pc_filtered;
63  pcl::PointCloud<pcl::PointXYZ> pc_projected;
64  tf::StampedTransform transform;
65 
66  //p_listener->waitForTransform(pcmsg_in.header.frame_id, ros::names::remap("/tracking_frame"),ros::Time::now(), ros::Duration(0.2));
67  try
68  {
69  //p_listener->waitForTransform(pc_frame_id, ros::names::remap("/tracking_frame"),ros::Time::now(), ros::Duration(0.2));
70  p_listener->lookupTransform(pc_frame_id, ros::names::remap("/tracking_frame"), ros::Time(0), transform);
71  //p_listener->lookupTransform(pcmsg_in.header.frame_id, ros::names::remap("/tracking_frame"), scan_in->header.stamp, transform);
72  }
73  catch (tf::TransformException ex)
74  {
75  ROS_ERROR("%s",ex.what());
76  }
77 
78 
79  pcl_ros::transformPointCloud(*pc_in,pc_transformed,transform.inverse());
80  pc_transformed.header.frame_id = ros::names::remap("/tracking_frame");
81  //pc_transformed.header.stamp = scan_in->header.stamp;
82 
83  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> epp;
84  pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud_constptr;
85  input_cloud_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (pc_transformed));
86  pcl::PointCloud<pcl::PointXYZ>::ConstPtr convex_hull_constptr;
87  convex_hull_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (convex_hull));
88 
89  //pcl::PointIndices::Ptr ind = pcl::PointIndices::Ptr(new pcl::PointIndices);
90  pcl::ExtractIndices<pcl::PointXYZ> extract; //Create the extraction object
91  pcl::PointIndices::Ptr indices;
92  indices.reset();
93  indices = pcl::PointIndices::Ptr(new pcl::PointIndices);
94 
95  //Set epp parameters
96  epp.setInputCloud(input_cloud_constptr);
97  epp.setInputPlanarHull(convex_hull_constptr);
98  epp.setHeightLimits(-perpendicular_treshold, perpendicular_treshold);
99  epp.setViewPoint(0,0,0); //i dont think this serves any purpose in the case of epp
100  epp.segment(*indices);
101 
102  extract.setInputCloud(pc_transformed.makeShared());
103  extract.setIndices(indices);
104  extract.setNegative(false);
105  extract.filter(pc_filtered);
106 
107  pcl::ProjectInliers<pcl::PointXYZ> projection;
108  projection.setModelType(pcl::SACMODEL_PLANE); //set model type
109  pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
110  coeff->values.resize(4);
111  coeff->values[0] = 0;
112  coeff->values[1] = 0;
113  coeff->values[2] = 1;
114  coeff->values[3] = 0;
115 
116  projection.setInputCloud(pc_filtered.makeShared());
117  projection.setModelCoefficients(coeff);
118  //projection.filter(pc_projected);
119 
120  coeff.reset();
121  input_cloud_constptr.reset();
122  convex_hull_constptr.reset();
123  indices.reset();
124 
125 // pc_accumulated += pc_projected;
126  pc_accumulated += pc_transformed;
127 
128 // #if _USE_DEBUG_
129 // ROS_INFO("pc_transformed has %d points", (int)pc_transformed.size());
130 // ROS_INFO("pc_filtered has %d points", (int)pc_filtered.size());
131 // ROS_INFO("pc_accumulated now has %d points", (int)pc_accumulated.size());
132 // #endif
133 }
134 
135 void scan_0_cb (const sensor_msgs::LaserScan::ConstPtr& scan_in)
136 {
137  laser_geometry::LaserProjection projector;
138  sensor_msgs::PointCloud2 pcmsg_in;
139 
140  projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,*p_listener); // não é necessário
141  pcmsg_in.header.stamp=ros::Time();
142  pcmsg_in.header.frame_id=scan_in->header.frame_id;
143 
144  pcl::PointCloud<pcl::PointXYZ> pc_in;
145  pcl::PCLPointCloud2 pcl_pc;
146  pcl_conversions::toPCL(pcmsg_in, pcl_pc);
147  pcl::fromPCLPointCloud2(pcl_pc, pc_in);
148 
149  pcl::fromROSMsg(pcmsg_in,pc_in);
150 
151  filter_cloud(&pc_in, scan_in->header.frame_id);
152 
153  laserscan_0_arrived=true;
154 }
155 
156 int main(int argc, char **argv)
157 {
158  ros::init(argc, argv, "simple_planar_pc_generator_node");
159  ros::NodeHandle n("~");
160  tf::TransformListener listener(n,ros::Duration(10));
161  p_listener=&listener;
162  p_n=&n;
163 
164  n.param("output_frequency", output_freq, 200.0);
165  n.param("perpendicular_treshold", perpendicular_treshold, 0.1);
166 
167  //Test the remapping of /tracking_frame
168  if (ros::names::remap("/tracking_frame")=="/tracking_frame")
169  {
170  ROS_ERROR("/tracking_frame was not remapped. Aborting program.");
171  ros::shutdown();
172  }
173 
174  pc_accumulated.header.frame_id = ros::names::remap("/tracking_frame");
175 
176  ros::Subscriber sub_0 = n.subscribe ("/laserscan0", 1, scan_0_cb);
177 
178  //declare the publisher
179  ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>("/pc_out", 1);
180 
181  //compute the convex hull to use in extract polygonal prism data
182  //Its just a very large square (500m*2 of side) around the /tracking_frame origin
183  convex_hull.header.frame_id = ros::names::remap("/tracking_frame");
184  pcl::PointXYZ pt;
185  pt.x = -500; pt.y=-500; pt.z=0;
186  convex_hull.points.push_back(pt);
187 
188  pt.x = 500; pt.y=-500; pt.z=0;
189  convex_hull.points.push_back(pt);
190 
191  pt.x = 500; pt.y= 500; pt.z=0;
192  convex_hull.points.push_back(pt);
193 
194  pt.x = -500; pt.y=500; pt.z=0;
195  convex_hull.points.push_back(pt);
196 
197  ros::Rate loop_rate(output_freq);
198 
199  while (n.ok())
200  {
201  ros::spinOnce();
202 
204  {
205  laserscan_0_arrived=false;
206 
207  sensor_msgs::PointCloud2 pcmsg_out;
208  pcl::toROSMsg(pc_accumulated, pcmsg_out);
209  pcmsg_out.header.stamp = ros::Time::now();
210 // #if _USE_DEBUG_
211 // ROS_INFO("Publishing pc_accumulated with %d points", (int)pc_accumulated.size());
212 // #endif
213  pub.publish(pcmsg_out);
214  pc_accumulated.erase(pc_accumulated.begin(), pc_accumulated.end());
215  }
216 
217 
218  loop_rate.sleep();
219  }
220 }
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
pcl::PointCloud< pcl::PointXYZ > convex_hull
int main(int argc, char **argv)
ros::NodeHandle * p_n
tf::TransformListener * p_listener
void scan_0_cb(const sensor_msgs::LaserScan::ConstPtr &scan_in)
void filter_cloud(pcl::PointCloud< pcl::PointXYZ > *pc_in, std::string pc_frame_id)


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