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 <Eigen/Geometry> 
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <boost/thread.hpp>
#include <time.h>
//計時
clock_t  clock_start, clock_end;
std::vector<Eigen::Isometry3d> all_pose;
std::vector<double> all_pose_stamp;
std::vector<std::vector<pcl::PointXYZ>> all_points;
std::vector<double> 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::PointXYZ> pcl_cloud_temp;
    pcl::fromROSMsg(ros_cloud, pcl_cloud_temp);
    std::vector<pcl::PointXYZ> cloud_vec;

    for(int i=0;i<int(pcl_cloud_temp.points.size());i++)
    {
	pcl::PointXYZ temp_one_point;
	temp_one_point.x=pcl_cloud_temp.points[i].x;
	temp_one_point.y=pcl_cloud_temp.points[i].y;
	temp_one_point.z=pcl_cloud_temp.points[i].z;
	cloud_vec.push_back(temp_one_point);
    }
    all_points.push_back(cloud_vec);
    all_points_stamp.push_back(double(ros_cloud.header.stamp.sec));
    cloud_vec.clear();
}

void pcd_maker()
{
    int pcd_num=0;
    int state_pcd=2;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZ> ); 
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_filter( new pcl::PointCloud<pcl::PointXYZ> ); 
    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_num<int(all_points[pcd_num].size()); cloud_temp_num++ )
	    {
		Eigen::Vector3d point;  
		point[0] = all_points[pcd_num][cloud_temp_num].x;
		point[1] = all_points[pcd_num][cloud_temp_num].y; 
		point[2] = all_points[pcd_num][cloud_temp_num].z;
		Eigen::Vector3d pointWorld = T*point;

		pcl::PointXYZ p ;
		p.x = pointWorld[0];
		p.y = pointWorld[1];
		p.z = pointWorld[2];
		pointCloud->points.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<pcl::PointXYZ> 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'
           

但這樣資料會在儲存前堆在記憶體裡，如果不降采樣，記憶體就很吃緊，跑不了大資料，也很難加入實時優化。