【激光SLAM】Lego_loam使用教程

安装

https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

 

运行

roslaunch lego_loam run.launch 

 

录制bag

rosbag record -o out /laser_cloud_surround

注意：out是bag文件名称，/laser_cloud_surround是录制的话题，lego_loam包含许多话题，其中/laser_cloud_surround是记录着整个地图．

 

bag转pcd文件

rosrun pcl_ros bag_to_pcd  out.bag /laser_cloud_surround  ./pcd

注意：会运行完后会生成一个pcd文件夹，文件夹中可能包含许多的pcd，选取最后的pcd文件，假设最后的pcd文件是last.pcd．

 

在RVIZ中显示点云图

创建cpp文件

    #include<ros/ros.h>  
    #include<pcl/point_cloud.h>  
    #include<pcl_conversions/pcl_conversions.h>  
    #include<sensor_msgs/PointCloud2.h>  
    #include <pcl/common/transforms.h>
    #include<pcl/io/pcd_io.h>
      
    int main (int argc, char **argv)  
    {  
      ros::init (argc, argv, "orbslam");  
      ros::NodeHandle nh;  
      ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/orbslam2_with_kinect2/output", 10);  
      
      pcl::PointCloud<pcl::PointXYZ> cloud1,cloud2;  
      sensor_msgs::PointCloud2 output;  
      pcl::io::loadPCDFile ("/home/xavier/test_ws/last.pcd", cloud1);

      Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();


      //绕x轴旋转一个theta角
      transform_2.rotate(Eigen::AngleAxisf(1.570795, Eigen::Vector3f::UnitX()));

      //执行变换
      //pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudOut(new pcl::PointCloud<pcl::PointXYZ>());
      pcl::transformPointCloud(cloud1, cloud2, transform_2);


      pcl::toROSMsg(cloud2,output);// 转换成ROS下的数据类型 最终通过topic发布
	
      output.header.stamp=ros::Time::now();
      output.header.frame_id  ="/camera_rgb_frame";
	
      ros::Rate loop_rate(1);  
      while (ros::ok())  
      {  
        pcl_pub.publish(output);  
        ros::spinOnce();  
        loop_rate.sleep();  
      }  
      return 0;  
    }  

创建launch

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.1" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="/camera_rgb_frame" />

    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="50.0" />
    <param name="latch" value="true" />

    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1000" />
    <param name="pointcloud_min_z" value="-1000" />

    <param name="ground_filter_angle" value="3.14" />

    <!-- topic from where pointcloud2 messages are subscribed -->
    <remap from="cloud_in" to="/orbslam2_with_kinect2/output" />

  </node>

</launch>

点击add 按钮添加 "PointCloud2模块"
设置topic为 "/orbslam2_with_kinect2/output"
设置FixedFram为"camera_rgb_frame"

注意：参考过博主熊猫飞天https://blog.csdn.net/crp997576280/article/details/74605766#comments，发现运行的程序出来的图像和rviz网格是垂直的，显示出的２d栅格图有问题，故在博主原程序上做个点云图的旋转变换，现在可以正常显示出２d栅格图．