28 #include <pcl/filters/voxel_grid.h>
42 sensor_msgs::PointCloud2
voxel_filter(sensor_msgs::PointCloud2 msg_in,
float voxel_size)
44 ROS_ERROR(
"Voxel grid does not work with sensor_msgs::PointCloud2, convertion is needed\n");
67 tf::StampedTransform transform;
70 ROS_ERROR(
"Point cloud stamp not set\n");
72 pcl::PointCloud<pcl::PointXYZRGB> pcl_transformed;
74 bool have_transform=
false;
78 p_listener->lookupTransform(acc_frame_id,pcl_pc.header.frame_id , ros::Time::now(), transform);
79 ROS_ERROR(
"Point cloud stamp not set\n");
83 catch (tf::TransformException ex)
88 ROS_ERROR(
"Point cloud stamp not set\n");
90 if(
p_listener->waitForTransform(acc_frame_id,pcl_pc.header.frame_id,ros::Time::now(), ros::Duration(3.0)))
94 p_listener->lookupTransform(acc_frame_id,pcl_pc.header.frame_id , ros::Time::now(), transform);
96 ROS_ERROR(
"Point cloud stamp not set\n");
101 catch (tf::TransformException ex)
103 ROS_ERROR(
"Could not lookup transform after waiting 3 secs\n.%s",ex.what());
108 ROS_ERROR(
"Could find valid transform after waiting 3 secs\n.%s",ex.what());
112 if (have_transform==
false)
118 pcl_ros::transformPointCloud(pcl_pc,pcl_transformed,transform);
119 pcl_transformed.header.frame_id=acc_frame_id;
123 sensor_msgs::PointCloud2 pc_trans;
124 sensor_msgs::PointCloud2 pc_voxel;
160 for(
long int i=pcl_pc.points.size()-1; i>0; --i)
162 float dist_x, dist_y, dist_z, dist_norm;
164 dist_x=pcl_pc.points[i].x-X0;
165 dist_y=pcl_pc.points[i].y-Y0;
166 dist_z=pcl_pc.points[i].z-Z0;
168 dist_norm=sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
184 pcl_pc.erase(pcl_pc.points.begin()+i);
pcl::PointCloud< pcl::PointXYZRGB > pcl_pc_acc
int remove_points_from_pointcloud(pcl::PointCloud< pcl::PointXYZRGB > pcl_pc, float dist, std::string frame_id)
Function the removes points from cloud This function calculated the origin of the given accumulation ...
tf::TransformListener * p_listener
int pointcloud_accumulated(sensor_msgs::LaserScan &scan_in, std::string acc_frame_id)
Function that receive a LaserScan and Accumulation Frame It converts the laserScan to a pcl::PointXYZ...
sensor_msgs::PointCloud2 voxel_filter(sensor_msgs::PointCloud2 msg_in, float voxel_size)