34 #include <laser_geometry/laser_geometry.h>
36 #include <pcl/point_types.h>
37 #include <pcl/io/pcd_io.h>
38 #include <pcl/kdtree/kdtree_flann.h>
39 #include <pcl/surface/mls.h>
40 #include <pcl_conversions/pcl_conversions.h>
52 pcl::PointCloud<pcl::PointXYZ> pclCloud;
53 pcl::PCLPointCloud2 pcl_pc;
54 pcl_conversions::toPCL(cloud, pcl_pc);
55 pcl::fromPCLPointCloud2(pcl_pc, pclCloud);
60 for(uint i=0;i<pclCloud.points.size();i++)
61 file<<pclCloud.points[i].x<<
" "<<pclCloud.points[i].y<<
" ";
63 file<<cloud.header.stamp<<endl;
74 static bool init=
false;
79 file<<
"# ros-bag to log converter"<<endl;
80 file<<
"# Jorge Almeida"<<endl;
81 file<<
"# Formato do ficheiro:"<<endl;
82 file<<
"# cada linha representa um scan diferente"<<endl;
83 file<<
"# é assumido que todos os pontos num scan foram obtidos ao mesmo tempo"<<endl;
84 file<<
"# o instante de tempo a que o scan corresponde aparece sempre na ultima coluna de uma linha"<<endl;
85 file<<
"# todos os scans têm o mesmo número de pontos"<<endl;
86 file<<
"# os valores aqui apresentados correspondem às distancias em metros"<<endl;
87 file<<
"# os valores de distancia são sempre positivos, um valor negativo é uma medida falhada"<<endl;
88 file<<
"# as distancias correspondem a ângulos de medição consecutivos começando em angle_min e terminando em angle_max"<<endl;
89 file<<
"# os valores desses ângulos vêm em radianos"<<endl;
90 file<<
"# podes calcular a frequência dos scans usando o tempo de cada um, o valor de scan_time é o valor médio/ótimo (tempo entre scans)"<<endl;
91 file<<
"# angle_min: "<<scan.angle_min<<endl;
92 file<<
"# angle_max: "<<scan.angle_max<<endl;
93 file<<
"# scan_time: "<<scan.scan_time<<endl;
98 for(uint i=0;i<scan.ranges.size();i++)
100 if(scan.ranges[i]<min || scan.ranges[i]>max)
103 file<<scan.ranges[i]<<
" ";
105 file<<scan.header.stamp<<endl;
111 init(argc,argv,
"mht");
115 Subscriber laser_handler = nh.subscribe(
"/scan", 1000,
saveLaserScan);
116 Subscriber point_handler = nh.subscribe(
"/points", 1000,
savePointCloud2);
121 perror(
"Error deleting file" );
124 cout<<
"Subscribed to "<<names::remap(
"/scan")<<endl;
125 cout<<
"Subscribed to "<<names::remap(
"/points")<<endl;
127 cout<<
"Spinning"<<endl;
void savePointCloud2(const sensor_msgs::PointCloud2 &cloud)
void saveLaserScan(const sensor_msgs::LaserScan &scan)
int main(int argc, char **argv)