29 #include <pcl_ros/transforms.h>
30 #include <pcl/conversions.h>
31 #include <pcl/point_cloud.h>
32 #include <pcl/point_types.h>
33 #include <laser_geometry/laser_geometry.h>
34 #include <sensor_msgs/PointCloud2.h>
35 #include <sensor_msgs/LaserScan.h>
36 #include <tf/transform_listener.h>
37 #include <nav_msgs/Odometry.h>
38 #include <pcl_conversions/pcl_conversions.h>
80 laser_geometry::LaserProjection projector_;
81 pcl::PointCloud<pcl::PointXYZ> pc_tmp;
82 pcl::PointCloud<pcl::PointXYZRGB> pc_laser;
83 sensor_msgs::PointCloud2 poincloud_in;
85 projector_.transformLaserScanToPointCloud(scan_in.header.frame_id,scan_in,poincloud_in,*p_listener);
88 pcl::PCLPointCloud2 pcl_pc;
89 pcl_conversions::toPCL(poincloud_in, pcl_pc);
90 pcl::fromPCLPointCloud2(pcl_pc, pc_tmp);
92 pcl::fromROSMsg(poincloud_in,pc_tmp);
95 pc_laser.points.resize(pc_tmp.size());
97 for (
size_t i = 0; i < pc_laser.points.size(); i++)
100 pc_laser.points[i].x = pc_tmp.points[i].x;
101 pc_laser.points[i].y = pc_tmp.points[i].y;
102 pc_laser.points[i].z = pc_tmp.points[i].z;
103 pc_laser.points[i].r = 0.0;
104 pc_laser.points[i].g = 0.0;
105 pc_laser.points[i].b = 0.0;
107 pc_laser.header.frame_id = scan_in.header.frame_id;
109 std::cout<<
"Message stamp not set!! please correct"<<std::endl;
112 return pointcloud_accumulated(pc_laser,acc_frame_id);
123 pcl::PointCloud<pcl::PointXYZRGB> pcl_pc;
124 for (
size_t i=0; i <pc_in.fields.size();i++)
126 if (pc_in.fields[i].name==
"rgb")
128 ROS_ERROR(
"TEM RGB");
130 pcl::fromROSMsg(pc_in,pcl_pc);
135 pcl::PointCloud<pcl::PointXYZ> pcl_tmp;
136 pcl::fromROSMsg(pc_in,pcl_tmp);
137 pcl_pc.points.resize(pcl_tmp.size());
138 for (
size_t i = 0; i < pcl_pc.points.size(); i++)
140 pcl_pc.points[i].x = pcl_tmp.points[i].x;
141 pcl_pc.points[i].y = pcl_tmp.points[i].y;
142 pcl_pc.points[i].z = pcl_tmp.points[i].z;
143 pcl_pc.points[i].r = 0.0;
144 pcl_pc.points[i].g = 0.0;
145 pcl_pc.points[i].b = 0.0;
150 pcl_pc.header.frame_id=pc_in.header.frame_id;
151 return pointcloud_accumulated(pcl_pc,acc_frame_id);
154 int pointcloud_accumulated(pcl::PointCloud<pcl::PointXYZRGB> pcl_pc,std::string acc_frame_id);
166 tf::StampedTransform transform;
170 std::cout<<
"Message stamp not set!! please correct"<<std::endl;
171 p_listener->lookupTransform(frame_id,_acc_frame_id , ros::Time::now(), transform);
174 catch (tf::TransformException ex)
176 ROS_ERROR(
"%s",ex.what());
180 double X0 = transform.getOrigin().x();
181 double Y0 = transform.getOrigin().y();
182 double Z0 = transform.getOrigin().z();
184 return remove_points_from_pointcloud(pcl_pc,dist, X0, Y0, Z0);
187 int remove_points_from_pointcloud(pcl::PointCloud<pcl::PointXYZRGB> pcl_pc,
float dist,
float X0,
float Y0,
float Z0);
std::string _acc_frame_id
nav_msgs::Odometry odometry_
sensor_msgs::PointCloud2 msg_cloud
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...
int pointcloud_accumulated(sensor_msgs::PointCloud2 &pc_in, std::string acc_frame_id)
Function that receive a PointCloud2 and Accumulation Frame If PointCloud2 has no field "rgb" then it ...