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>
86 if (ros::names::remap(
"/tracking_frame")==
"/tracking_frame")
88 ROS_ERROR(
"/tracking_frame was not remapped. Aborting program.");
92 pc_accumulated.header.frame_id = ros::names::remap(
"/tracking_frame");
103 convex_hull.header.frame_id = ros::names::remap(
"/tracking_frame");
105 pt.x = -500; pt.y=-500; pt.z=0;
108 pt.x = 500; pt.y=-500; pt.z=0;
111 pt.x = 500; pt.y= 500; pt.z=0;
114 pt.x = -500; pt.y=500; pt.z=0;
118 void filterCloud(pcl::PointCloud<pcl::PointXYZ>* pc_in, std::string pc_frame_id)
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;
127 transform_listener.lookupTransform(pc_frame_id, ros::names::remap(
"/tracking_frame"), ros::Time(0), transform);
130 catch (tf::TransformException ex)
132 ROS_ERROR(
"%s",ex.what());
136 pcl_ros::transformPointCloud(*pc_in,pc_transformed,transform.inverse());
137 pc_transformed.header.frame_id = ros::names::remap(
"/tracking_frame");
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));
147 pcl::ExtractIndices<pcl::PointXYZ> extract;
148 pcl::PointIndices::Ptr indices;
150 indices = pcl::PointIndices::Ptr(
new pcl::PointIndices);
153 epp.setInputCloud(input_cloud_constptr);
154 epp.setInputPlanarHull(convex_hull_constptr);
156 epp.setViewPoint(0,0,0);
157 epp.segment(*indices);
159 extract.setInputCloud(pc_transformed.makeShared());
160 extract.setIndices(indices);
161 extract.setNegative(
false);
162 extract.filter(pc_filtered);
164 pcl::ProjectInliers<pcl::PointXYZ> projection;
165 projection.setModelType(pcl::SACMODEL_PLANE);
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;
173 projection.setInputCloud(pc_filtered.makeShared());
174 projection.setModelCoefficients(coeff);
175 projection.filter(pc_projected);
178 input_cloud_constptr.reset();
179 convex_hull_constptr.reset();
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());
193 laser_geometry::LaserProjection projector;
194 sensor_msgs::PointCloud2 pcmsg_in;
196 projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,
transform_listener);
197 pcmsg_in.header.stamp=ros::Time();
198 pcmsg_in.header.frame_id=scan_in->header.frame_id;
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);
214 laser_geometry::LaserProjection projector;
215 sensor_msgs::PointCloud2 pcmsg_in;
217 projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,
transform_listener);
218 pcmsg_in.header.stamp=ros::Time();
219 pcmsg_in.header.frame_id=scan_in->header.frame_id;
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);
244 sensor_msgs::PointCloud2 pcmsg_out;
246 pcmsg_out.header.stamp = ros::Time::now();
249 ROS_INFO(
"Publishing pc_accumulated with %d points", (
int)
pc_accumulated.size());
260 int main(
int argc,
char **argv)
262 ros::init(argc, argv,
"simple_planar_pc_generator_node");
263 ros::NodeHandle n(
"~");
void scan1Handler(const sensor_msgs::LaserScan::ConstPtr &scan_in)
void filterCloud(pcl::PointCloud< pcl::PointXYZ > *pc_in, std::string pc_frame_id)
pcl::PointCloud< pcl::PointXYZ > convex_hull
ros::Subscriber scan_1_subscriber
int main(int argc, char **argv)
tf::TransformListener transform_listener
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
PointCloudGeneretor(ros::NodeHandle nh_)
ros::Publisher points_publisher
double perpendicular_treshold
ros::Subscriber scan_2_subscriber
void scan2Handler(const sensor_msgs::LaserScan::ConstPtr &scan_in)