博客

一.环境配置

1.见上一篇博客，跑通RS_LIDAR_SDK，不需要转换VELODYNE格式。

二.LEGO-LOAM

1.github自己找源码，运行的时候有一个bug，见这篇文章解决，谢谢作者大大。

3、速腾16线激光雷达RS-16 —– 3D建图算法LeGO-LOAM的使用（Ubuntu18.04 + ROS Melodic）_legoloam实时建图_杰尼君的博客-CSDN博客

2.源码有问题不会改github找一下，直接解决

https://github.com/ZYCheng-coder/lego-loam-ws

需要修改的几个地方 注意一下：

extern const string pointCloudTopic = "/rslidar_points";  // /velodyne_points
extern const string imuTopic = "/imu/data";

// Save pcd
extern const string fileDirectory = "/tmp/";

// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used

这里要注意：直接使用的是雷达输出数据话题

其次要注意的是，不使用CloudRing 因为源码的点云格式是XYZIR

懂得都懂 不多解释

        // have "ring" channel in the cloud -- false
        if (useCloudRing == true)
        {
            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
            if (laserCloudInRing->is_dense == false) {
                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
                ros::shutdown();
            }  
        }

下面要注意的是launch文件：

懂得都懂，不多解释

<launch>
    
    <!--- Sim Time -->
    <param name="/use_sim_time" value="false" />

    <!--- Run Rviz-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lego_loam)/launch/test.rviz" />

    <!--- TF -->
    <node pkg="tf" type="static_transform_publisher" name="camera_init_to_map"  args="0 0 0 1.570795   0        1.570795 /map    /camera_init 10" />
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0        /camera /base_link   10" />

    <!--- LeGO-LOAM -->    
    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen">
    <param name="lidarType" type="string" value="rslidar" />
    </node>
    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="lego_loam" type="mapOptmization"     name="mapOptmization"     output="screen"/>
    <node pkg="lego_loam" type="transformFusion"    name="transformFusion"    output="screen"/>

</launch>

三.保存TUM格式轨迹

        std::ofstream foutC("/home/lwj/lego_second_txt/college.txt", std::ios::app);
        foutC.setf(std::ios::fixed, std::ios::floatfield);
        foutC.precision(6);
        foutC << laserOdometry2.header.stamp << " "
              << laserOdometry2.pose.pose.position.x << " "
              << laserOdometry2.pose.pose.position.y << " "
              << laserOdometry2.pose.pose.position.z << " "
              << laserOdometry2.pose.pose.orientation.x << " "
              << laserOdometry2.pose.pose.orientation.y << " "
              << laserOdometry2.pose.pose.orientation.z << " "
              << laserOdometry2.pose.pose.orientation.w << std::endl;
        foutC.close();

完美结束！