ros下loam保存数据，生成pcd文件[附代码和数据]

最近项目比较忙没空更新博客，发现问保存LOAM生成点云数据的小伙伴挺多，现写个博客说一下。

方法一。可以改loam程序，一段时间不再输入数据则保存pcd。

方法二。可以使用ros实时录包。

两个方法本质一样，都是提取每帧位姿信息和点云信息，之后pcl填充点云，如果连续几秒没新数据就生成pcd。介绍一下我写的方法二。

首先下载loam，我使用的https://github.com/cuitaixiang/LOAM_NOTED也就是源LOAM程序，编译并添加源到.bashrc文件。

依次开四个cmd分别输入运行。ros核，录包，运行loam，输入数据（记得改路径）

数据：

链接: https://pan.baidu.com/s/1GaZ2eGZdfc-cluSc-bkQng 提取码: 9zsp

roscore

rosbag record /integrated_to_init /velodyne_cloud_3

roslaunch loam_velodyne loam_velodyne.launch

rosbag play '/home/eminbogen/data/nsh_indoor_outdoor.bag'

 这样一个有每帧位姿和点云的rosbag就生成了，之后用catkin_make编译一个拼接程序，记得添加ros源到.bashrc里。

cmakelist：

cmake_minimum_required(VERSION 2.8.3)
project(rosbag_make)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")

#find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
  message_generation
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
   CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
   INCLUDE_DIRS include
)

include_directories(include ${catkin_INCLUDE_DIRS} "/usr/local/include/eigen3" "/home/eminbogen/pkg/lastootsLAStools/src" ${PCL_INCLUDE_DIRS})

add_executable(pcd_get src/pcd_get.cpp)
target_link_libraries(pcd_get ${catkin_LIBRARIES} ${PCL_LIBRARIES})

pcd_get.cpp：

pcd保存路径要改一下，如果你跑的不是loam录制topic应该要更改。

#include  
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
//计时
clock_t  clock_start, clock_end;
std::vector all_pose;
std::vector all_pose_stamp;
std::vector> all_points;
std::vector all_points_stamp;

void odomeCallback(const nav_msgs::Odometry laserOdometry)
{
    Eigen::Quaterniond q( laserOdometry.pose.pose.orientation.w, 
			  laserOdometry.pose.pose.orientation.x, 
			  laserOdometry.pose.pose.orientation.y, 
			  laserOdometry.pose.pose.orientation.z );
    Eigen::Isometry3d T(q);
    T.pretranslate( Eigen::Vector3d( laserOdometry.pose.pose.position.x, 
				     laserOdometry.pose.pose.position.y, 
				     laserOdometry.pose.pose.position.z ));
    all_pose.push_back( T );
    all_pose_stamp.push_back(double(laserOdometry.header.stamp.sec));
}

void cloudCallback(const sensor_msgs::PointCloud2 ros_cloud)
{
    //ROS_INFO("cloud is going");
    pcl::PointCloud pcl_cloud_temp;
    pcl::fromROSMsg(ros_cloud, pcl_cloud_temp);
    std::vector cloud_vec;

    for(int i=0;i::Ptr pointCloud( new pcl::PointCloud ); 
    pcl::PointCloud::Ptr pointCloud_filter( new pcl::PointCloud ); 
    ROS_INFO("pcd_all_begin");
    while(state_pcd)
    {
	ROS_INFO("pcd_num=%d",pcd_num);
	if((int(all_points_stamp.size())-10>pcd_num)&&(int(all_pose_stamp.size())-10>pcd_num*2))
	{   
	    if(state_pcd==2){state_pcd=1;}
	    Eigen::Isometry3d T = all_pose[pcd_num*2];
	    for ( int cloud_temp_num=0; cloud_temp_numpoints.push_back( p );
	    }
	    pcd_num++;
	    clock_start=clock();
	}
	clock_end=clock();
	if(state_pcd==1&&double(clock_end-clock_start)/ CLOCKS_PER_SEC>5){state_pcd=0;}
    }
    pointCloud->is_dense = false;
    pcl::VoxelGrid sor;
    sor.setInputCloud (pointCloud);
    sor.setLeafSize (0.2, 0.2, 0.2);
    sor.filter (*pointCloud_filter);
    pcl::io::savePCDFileBinary("/home/eminbogen/my_ros/map.pcd", *pointCloud_filter );
}

int main(int argc, char **argv)
{ 
    ros::init(argc, argv, "pcd_get");
    ros::NodeHandle n;
    ROS_INFO("begin");
    ros::Subscriber odome_sub = n.subscribe("/integrated_to_init", 50, odomeCallback);
    ros::Subscriber cloud_sub = n.subscribe("/velodyne_cloud_3", 50, cloudCallback);
    boost::thread server(pcd_maker);
    ros::spin();
    return 0;
}

依次打开三个cmd运行。改路径！

roscore

rosrun rosbag_make pcd_get

rosbag play '/home/eminbogen/my_ros/2020-07-10-18-27-27.bag'

但这样数据会在保存前堆在内存里，如果不降采样，内存就很吃紧，跑不了大数据，也很难加入实时优化。