速腾聚创16线雷达离线和实时跑Lego-loam

记录一下自己跑上届学长的legoloam功能包的过程，以及踩过的坑，有错误还望指出

一：离线跑自己录制的点云包

如果你是速腾聚创16线雷达，就直接使用这个run.launch文件吧

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

    <param name="/use_sim_time" value="true" />
    <node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
    
    <!-- Run a passthrough filter to clean NaNs -->
    <node pkg="nodelet" type="nodelet" name="passthrough" args="load pcl/PassThrough pcl_manager" output="screen">
        <remap from="~input" to="/rslidar_points" />
        <remap from="/passthrough/output" to="/velodyne_points" />
        
        <rosparam>
        filter_field_name: z
        filter_limit_negative: True
        </rosparam>

    <rosparam>
        filter_field_name: x
        filter_limit_negative: True
        </rosparam>

    <rosparam>
        filter_field_name: y
        filter_limit_negative: True
       
        </rosparam>
    </node>

    <!--- 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="my_slam" type="imageProjection"    name="imageProjection"    output="screen"/>
    <node pkg="my_slam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="my_slam" type="mapOptmization"     name="mapOptmization"     output="screen"/>
    <node pkg="my_slam" type="transformFusion"    name="transformFusion"    output="screen"/>




</launch>

其中这一部分是去除NAN点云

<!-- Run a passthrough filter to clean NaNs -->
    <node pkg="nodelet" type="nodelet" name="passthrough" args="load pcl/PassThrough pcl_manager" output="screen">
        <remap from="~input" to="/rslidar_points" />
        <remap from="/passthrough/output" to="/velodyne_points" />

如果不加这一部分的话，会出现如下问题

featureAssociation: /home/chenyi/pcl-1.9/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, st
d::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZI; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Inval
id (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
[featureAssociation-6] process has died [pid 30877, exit code -6, cmd /home/chenyi/legoloam_ws/devel/lib/lego_loam/featureAssociation __name:=featureAssociation __log:
=/home/chenyi/.ros/log/c90973c6-82f3-11e9-a7c2-2c4d5498d09e/featureAssociation-6.log].
log file: /home/chenyi/.ros/log/c90973c6-82f3-11e9-a7c2-2c4d5498d09e/featureAssociation-6*.log

最后在跑包时的命令如下

rosbag play xxx.bag --clock

不要忘记加后边的 --clock

二：实时线上跑包

第一步

将刚才发过的legoloam的launch文件中的true改为false，如下所示

<param name="/use_sim_time" value="false" />

第二步

在速腾聚创雷达的驱动的launch文件中加入映射，如下

<remap from="/rslidar_points" to="/velodyne_points" />

第三步

先运行雷达的launch，再运行legoloam的launch

完成起飞