36 PointCloud<PointXYZ> laser_cloud;
37 sensor_msgs::PointCloud2 laser;
38 laser.header.frame_id=msg->header.frame_id;
39 tf::TransformListener listener;
41 tf::StampedTransform transform;
46 catch (tf::TransformException ex)
48 ROS_ERROR(
"%s",ex.what());
51 laser_geometry::LaserProjection projector_;
52 projector_.transformLaserScanToPointCloud(msg->header.frame_id,*msg,laser,listener);
54 fromROSMsg(laser,laser_cloud);
55 laser_cloud.header.frame_id=msg->header.frame_id;
58 PointCloud<PointXYZ> laser_cloud_transformed;
59 pcl_ros::transformPointCloud (laser_cloud,laser_cloud_transformed,transform);
62 points_from_volume<PointXYZ> pfv;
63 PointCloud<PointXYZ> cloud_cut;
66 laser_cloud_transformed.header.frame_id=
frame_id;
68 pfv.convexhull_function(laser_cloud_transformed,
max_z,
min_z,
false);
69 cloud_cut=pfv.get_pc_in_volume();
71 sensor_msgs::PointCloud2 msg_laser;
72 toROSMsg(cloud_cut, msg_laser);
tf::TransformListener * listener_ptr
transform_listener pointer
PointCloud< PointXYZ > convex_hull
convex_hull for extraction
laser_to_pc class declaration
double min_z
z limits for extraction
ros::Publisher * pub_ptr
publisher pointer
void filter(const sensor_msgs::LaserScanPtr &msg)
callback function