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>
59 void filter_cloud(pcl::PointCloud<pcl::PointXYZ>* pc_in, std::string pc_frame_id)
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;
70 p_listener->lookupTransform(pc_frame_id, ros::names::remap(
"/tracking_frame"), ros::Time(0), transform);
73 catch (tf::TransformException ex)
75 ROS_ERROR(
"%s",ex.what());
79 pcl_ros::transformPointCloud(*pc_in,pc_transformed,transform.inverse());
80 pc_transformed.header.frame_id = ros::names::remap(
"/tracking_frame");
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));
90 pcl::ExtractIndices<pcl::PointXYZ> extract;
91 pcl::PointIndices::Ptr indices;
93 indices = pcl::PointIndices::Ptr(
new pcl::PointIndices);
96 epp.setInputCloud(input_cloud_constptr);
97 epp.setInputPlanarHull(convex_hull_constptr);
99 epp.setViewPoint(0,0,0);
100 epp.segment(*indices);
102 extract.setInputCloud(pc_transformed.makeShared());
103 extract.setIndices(indices);
104 extract.setNegative(
false);
105 extract.filter(pc_filtered);
107 pcl::ProjectInliers<pcl::PointXYZ> projection;
108 projection.setModelType(pcl::SACMODEL_PLANE);
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;
116 projection.setInputCloud(pc_filtered.makeShared());
117 projection.setModelCoefficients(coeff);
121 input_cloud_constptr.reset();
122 convex_hull_constptr.reset();
135 void scan_0_cb (
const sensor_msgs::LaserScan::ConstPtr& scan_in)
137 laser_geometry::LaserProjection projector;
138 sensor_msgs::PointCloud2 pcmsg_in;
140 projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,*
p_listener);
141 pcmsg_in.header.stamp=ros::Time();
142 pcmsg_in.header.frame_id=scan_in->header.frame_id;
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);
149 pcl::fromROSMsg(pcmsg_in,pc_in);
156 int main(
int argc,
char **argv)
158 ros::init(argc, argv,
"simple_planar_pc_generator_node");
159 ros::NodeHandle n(
"~");
160 tf::TransformListener listener(n,ros::Duration(10));
168 if (ros::names::remap(
"/tracking_frame")==
"/tracking_frame")
170 ROS_ERROR(
"/tracking_frame was not remapped. Aborting program.");
174 pc_accumulated.header.frame_id = ros::names::remap(
"/tracking_frame");
176 ros::Subscriber sub_0 = n.subscribe (
"/laserscan0", 1,
scan_0_cb);
179 ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>(
"/pc_out", 1);
183 convex_hull.header.frame_id = ros::names::remap(
"/tracking_frame");
185 pt.x = -500; pt.y=-500; pt.z=0;
188 pt.x = 500; pt.y=-500; pt.z=0;
191 pt.x = 500; pt.y= 500; pt.z=0;
194 pt.x = -500; pt.y=500; pt.z=0;
207 sensor_msgs::PointCloud2 pcmsg_out;
209 pcmsg_out.header.stamp = ros::Time::now();
213 pub.publish(pcmsg_out);
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
pcl::PointCloud< pcl::PointXYZ > convex_hull
int main(int argc, char **argv)
tf::TransformListener * p_listener
double perpendicular_treshold
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)