Autoware 1.12学习整理--02--构建点云地图

〇、前言

构建点云地图可以使用真实的激光雷达对真实环境构建点云地图，也可以使用仿真的激光雷达对仿真环境构建点云地图，后者通常使用Lgsvl与autoware进行联合仿真，Lgsvl是一款专业的自动驾驶仿真软件，但本人对gazebo更熟悉，所以本文使用Ndt_mapping模块构建gazebo环境的点云地图。

一、配置gazebo仿真环境

1.1、下载仿真功能包

cd your_workspace/src/
git clone https://github.com/yukkysaito/vehicle_sim.git --recurse-submodules

1.2、编译功能包

cd your_workspace
catkin_make

1.3、测试

roslaunch vehicle_sim_launcher world_test.launch

运行上述指令后，打开了gazebo仿真环境，Rviz界面和rqt_robot_steering插件，如下图所示，可以通过rqt_robot_steering插件控制仿真环境中的汽车移动。

二、构建点云地图

2.1、录制点云数据
autoware默认的激光雷达坐标系是velodyne，话题名称是/points_raw，要确保仿真环境中的与autoware的一致。打开1.3小节的仿真环境，使用以下指令录制点云数据，使用rqt_robot_steering插件控制汽车在仿真环境中走一圈，最后Ctrl+C停止录制。

rosbag recored /points_raw

也可以点击autoware界面的ROSBAG按钮，在弹出的对话框中点击下方Refresh 按钮刷新话题列表，找到/points_raw并勾选，点击Start 按钮开始录制数据，汽车走完一圈后点击Stop停止录制。


2.2、查看点云数据
在建图前，可以使用如下指令播放记录的点云数据，将play后面的换成自己的rosbag数据名称。

rosbag play 2022-03-03-22-10-39.bag

也可以通过autoware界面播放录制的点云数据，点击Simulation页面，点击Ref按钮加载录制的bag文件，将Start Time设置成0，点击Play等到界面出现bag文件信息后，点击Pause按钮停止播放，等打开Rviz后再播放，如下图所示：

打开Rviz，将Fixed Frame由world修改为velodyne，点击左下角Add按钮，加载By Topic下/points_raw话题的 PointCloud2 选项，此时能够看到点云数据：

2.3、建图
建图时需要完成如下图所示的tf树建立，从world到map的坐标系转换与从base_link到velodyne的坐标系转换是固定不变的，用ROS的TF工具即可，map到base_link的坐标系转换由autoware中的ndt_mapping模块完成。

2.3.1、完成从base_link到velodyne的坐标系转换。
切换到autoware的Setup页面下，确定Localizer选的是Velodyne，设置Baselink to Localizer的x、y、z、yaw、pitch、roll数值，其为雷达中心点与车身后轴中心点的相对位置关系，接着点击TF按钮，最后点击Vehicle Model按钮，如果其后为空，那么会加载一个默认模型。

2.3.2、完成从world到map的坐标系转换。
切换到autoware的Map页面下，点击TF右侧的Ref按钮，加载如下路径的launch文件，其中是从/world坐标系转换到/map坐标系的tf变换，最后点击TF按钮。

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch

2.3.3、完成map到base_link的坐标系转换
切换到autoware的Computing页面下，点击lidar_localizer下nat_mapping右侧的app按钮，参数用默认的即可，Method Type选择pcl_anh_gpu，即使用GPU进行建图计算，点击Close按钮，最后勾选nat_mapping。

2.3.4、播放数据进行建图
和2.2小节中在autoware界面播放录制的点云数据一样，点击进入Simulaton页面，点击Ref按钮加载之前录制的bag文件，点击Play按钮播放数据，然后再点击Pause暂停播放。点击界面右下方的Rviz按钮，启动Rviz可视化界面，加载Rviz的配置文件ndt_mapping.rviz，但是1.12版本并不存在这个文件，可以在任意位置新建一个ndt_mapping.rviz文件，用gedit打开，输入以下内容并保存：

Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
      Splitter Ratio: 0.5
    Tree Height: 870
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.588679
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: ""
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.03
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        base_link:
          Value: true
        map:
          Value: true
        velodyne:
          Value: true
        world:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        map:
          base_link:
            velodyne:
              {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/PointCloud2
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 254
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: NDT Map
      Position Transformer: XYZ
      Queue Size: 1
      Selectable: true
      Size (Pixels): 1
      Size (m): 0.01
      Style: Points
      Topic: /ndt_map
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: Vehicle Model
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: map
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Topic: /initialpose
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Angle: 0
      Class: rviz/TopDownOrtho
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.06
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Name: Current View
      Near Clip Distance: 0.01
      Scale: 10
      Target Frame: <Fixed Frame>
      Value: TopDownOrtho (rviz)
      X: 0
      Y: 0
    Saved: ~
Window Geometry:
  Camera:
    collapsed: false
  Displays:
    collapsed: false
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003f5fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003f5000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003a90000008d0000000000000000fb0000000c00430061006d0065007200610000000420000000160000001600000016000000010000010f000003f5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc00000041000003f5000000b000fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0056006900650077007301000003a10000010f0000010f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007510000003efc0100000002fb0000000800540069006d0065010000000000000751000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc000003f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false

加载rviz的配置文件后，回到autoware的Simulaton页面，再次点击Pause按钮开始播放，这时可以看到Rviz可视化界面显示如下建图过程：

在建图过程中，终端会输出下图所示的信息，其中(Process/Input): (555 / 555)，这两个数字前一个数字表示已经处理的点云帧数，后一个表示加载的点云帧数，当两个数字相差过大时，点击Simulaton页面的Pause按钮停止播放数据，等待数据处理，当两个数字重新接近之后，可以再次点击Pause按钮继续运行。

2.3.5、保存地图
当bag数据播放完毕，建图结束后，点击nat_mapping右侧的app按钮，在打开的页面中点击Ref按钮，选择点云地图保存路径，将Filter Resolution设置为0.2，最后点击PCD OUTPUT按钮导出点云地图。

2.4、查看点云地图
打开autoware的Map菜单，点击Point Cloud按钮右侧的Ref按钮，加载刚才保存.pcd文件，并点击Point Cloud按钮，进度条显示OK，则加载完毕，如下所示：

接着打开rviz，在rviz显示界面点击左下角的Add按钮，通过By Topic找到/points_map并打开，将Fixed Frame设置为map，则在rviz中显示如下：

也可以使用pcl_viewer可以查看pcd点云地图，使用如下命令进行安装：

sudo apt-get install libpcl-dev
sudo ln -s /usr/lib/libvtkproj4.so.5.10 /usr/lib/libvtkproj4.so
sudo apt-get install pcl-tools

使用如下命令打开点云地图：

pcl_viewer xxxx.pcd

文章不妥之处还望指正

参考文章：
https://blog.csdn.net/Travis_X/article/details/105418119
https://www.cnblogs.com/hgl0417/p/11130747.html
https://zhuanlan.zhihu.com/p/164432779