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>
50 #define PFLN printf("file %s line %d\n",__FILE__,__LINE__);
63 void filter_cloud(pcl::PointCloud<pcl::PointXYZ>* pc_in, std::string pc_frame_id)
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;
73 p_listener->lookupTransform(pc_frame_id, ros::names::remap(
"/tracking_frame"), ros::Time(0), transform);
76 catch (tf::TransformException ex)
78 ROS_ERROR(
"%s",ex.what());
82 pcl_ros::transformPointCloud(*pc_in,pc_transformed,transform.inverse());
83 pc_transformed.header.frame_id = ros::names::remap(
"/tracking_frame");
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));
93 pcl::ExtractIndices<pcl::PointXYZ> extract;
94 pcl::PointIndices::Ptr indices;
96 indices = pcl::PointIndices::Ptr(
new pcl::PointIndices);
99 epp.setInputCloud(input_cloud_constptr);
100 epp.setInputPlanarHull(convex_hull_constptr);
102 epp.setViewPoint(0,0,0);
103 epp.segment(*indices);
105 extract.setInputCloud(pc_transformed.makeShared());
106 extract.setIndices(indices);
107 extract.setNegative(
false);
108 extract.filter(pc_filtered);
110 pcl::ProjectInliers<pcl::PointXYZ> projection;
111 projection.setModelType(pcl::SACMODEL_PLANE);
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;
119 projection.setInputCloud(pc_filtered.makeShared());
120 projection.setModelCoefficients(coeff);
121 projection.filter(pc_projected);
124 input_cloud_constptr.reset();
125 convex_hull_constptr.reset();
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());
137 void cloud_cb (
const sensor_msgs::PointCloud2ConstPtr & pcmsg_in)
141 ROS_INFO(
"Received point cloud");
143 pcl::PointCloud<pcl::PointXYZ> pc_in;
144 pcl::PCLPointCloud2 pcl_pc;
145 pcl_conversions::toPCL(*pcmsg_in, pcl_pc);
147 pcl::fromROSMsg(*pcmsg_in,pc_in);
151 void scan_cb (
const sensor_msgs::LaserScan::ConstPtr& scan_in)
154 ROS_INFO(
"Received laser scan");
158 laser_geometry::LaserProjection projector;
159 sensor_msgs::PointCloud2 pcmsg_in;
161 projector.transformLaserScanToPointCloud(scan_in->header.frame_id, *scan_in,pcmsg_in,*
p_listener);
162 pcmsg_in.header.stamp=ros::Time();
163 pcmsg_in.header.frame_id=scan_in->header.frame_id;
165 pcl::PointCloud<pcl::PointXYZ> pc_in;
166 pcl::fromROSMsg(pcmsg_in,pc_in);
172 int main(
int argc,
char **argv)
175 ros::init(argc, argv,
"planar_pc_generator_node");
176 ros::NodeHandle n(
"~");
177 tf::TransformListener listener(n,ros::Duration(10));
184 if (ros::names::remap(
"/tracking_frame")==
"/tracking_frame")
186 ROS_ERROR(
"/tracking_frame was not remapped. Aborting program.");
189 pc_accumulated.header.frame_id = ros::names::remap(
"/tracking_frame");
192 for (
int i=0; i<5; i++)
195 sprintf(str,
"/laserscan%d", i);
196 if (ros::names::remap(str)!=str)
203 for (
int i=0; i<5; i++)
206 sprintf(str,
"/pointcloud%d", i);
207 if (ros::names::remap(str)!=str)
213 std::vector<ros::Subscriber> sub;
216 for (
int i=0; i<5; i++)
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());
229 for (
int i=0; i<5; i++)
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());
243 ROS_ERROR(
"No /laserscan[0 4] or /pointcloud[0 4] where remapped. Aborting...");
248 ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>(
"/pc_out", 1);
252 convex_hull.header.frame_id = ros::names::remap(
"/tracking_frame");
254 pt.x = -500; pt.y=-500; pt.z=0;
257 pt.x = 500; pt.y=-500; pt.z=0;
260 pt.x = 500; pt.y= 500; pt.z=0;
263 pt.x = -500; pt.y=500; pt.z=0;
282 sensor_msgs::PointCloud2 pcmsg_out;
284 pcmsg_out.header.stamp = ros::Time::now();
286 ROS_INFO(
"Publishing pc_accumulated with %d points", (
int)
pc_accumulated.size());
288 pub.publish(pcmsg_out);
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)
pcl::PointCloud< pcl::PointXYZ > pc_accumulated
bool process_laser_scan[5]
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