Autoware.AIでVelodyne VLP-16様な高価な3D Lidarが必須ですが、安価な2D Lidar( RPLIDAR A3)を使用できるかを試します。

目次

動作環境

MacBookPro A1708 (Core i7, 16GB RAM, 512GB SSD)
AsRock DeskMini GTX1060(Z370)
Core i9-9900 (3.10GHz 8core 16thread)
メモリー32GB
NVIDIA GeForce GTX 1060 Mobile(6GB GDDR5)
Ubuntu 18.04.5
ROS Melodic
Qt Creator 4.9.2
Autoware 1.14.0
RPLIDAR A3

2D Lidar( RPLIDAR A3)のデータをROSBAGに保存

RPLIDAR ROS packageのインストール

cd ~/catkin_ws/src
git clone https://github.com/Slamtec/rplidar_ros.git
cd ~/catkin_ws
catkin_make

RPLIDAR ROS packageのnode.cppを改造

RPLIDARのscanデータと共にscanデータをPointCloud2形式に変換して出力するように修正します。

PointCloud2 Topicのframe_idとtopic_nameをAutowareに合わせて修正します。

frame_id : velodyne
topic_name : points_raw

RPLIDARのデータを記録

車の上に載せって走行したRPLIDARのScanデータをROSBAGに記録

$ rosbag record -a

$ rosbag info 2020-09-01-22-44-42.bag
path:        2020-09-01-22-44-42.bag
version:     2.0
duration:    1:14s (74s)
start:       Sep 01 2020 22:44:42.32 (1598967882.32)
end:         Sep 01 2020 22:45:56.63 (1598967956.63)
size:        240.8 MB
messages:    674885
compression: none [311/311 chunks]
types:       rosgraph_msgs/Log       [acffd30cd6b6de30f120938c17c593fb]
             sensor_msgs/LaserScan   [90c7ef2dc6895d81024acba2ac42f369]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:      /points_raw      984 msgs    : sensor_msgs/PointCloud2
             /rosout       336083 msgs    : rosgraph_msgs/Log       (3 connections)
             /rosout_agg   336834 msgs    : rosgraph_msgs/Log      
             /scan            984 msgs    : sensor_msgs/LaserScan

ROSBAGのPointCloud2データから地図を作成

BLAMで地図を作成します。

BLAMをインストール

Ubuntu18.04 でBLAMをインストールを参考してインストールします。

地図を作成

blam/internal/src/blam_example/launch/test_offline.launchでscan_topic名をROSBAGのtopic名に変更します。

velodyne_points → points_raw

$ cd blam
$ ./blam_offline.sh 2020-09-01-22-44-42.bag

NODES
  /blam/
    blam_slam (blam_slam/blam_slam_offline)

ROS_MASTER_URI=http://localhost:11311

process[blam/blam_slam-1]: started with pid [14250]
[ INFO] [1598973632.507347948]: /blam/blam_slam/BlamSlam: Registering log callbacks.
[ INFO] [1598973632.509999720]: STARTING LoadBagfile
[ INFO] [1598973632.510012012]: /blam/blam_slam/BlamSlamOffline: Processing the following topics:
	/points_raw
[ INFO] [1598973632.622944827]: /blam/blam_slam/BlamSlamOffline: Bag start time    = 1598967882.530024 	 Bag end time    = 1598967956.633434
[ INFO] [1598973632.622991533]: /blam/blam_slam/BlamSlamOffline: Replay start time =         0.000000 	 Replay end time = 4294967296.000000
[ INFO] [1598973632.661777337]: STARTING ProcessBagfile
[ INFO] [1598973632.661799079]: /blam/blam_slam/BlamSlamOffline: Sorting messages.
[ INFO] [1598973632.666309510]: /blam/blam_slam/BlamSlamOffline: Processing messages.
[ INFO] [1598973632.666323489]: Processing 1 out of Total 984

[ INFO] [1598973633.955234963]: /blam/blam_slam/BlamSlamOffline: Finished processing bag file, 5747.575541 percent of real-time.
[ INFO] [1598973634.997859155]: PCD file prefix is: blam/pcd_offline/map_
[ INFO] [1598973634.998685363]: Saving as ASCII PCD
[ INFO] [1598973634.999621759]: Listening for incoming data on topic /blam/blam_slam/octree_map
[ INFO] [1598973636.508294270]: Received 1394 data points in frame world with the following fields: x y z
[ INFO] [1598973636.508377475]: Data saved to blam/pcd_offline/map_0.pcd

作成した地図を確認

Autoware.AIでROSBAGから経路生成

Autoware.AIを起動

$ roslaunch runtime_manager runtime_manager.launch

ROSBAGをロード

ROSBAGを選択してplay → Pause で一時停止

Setup TabでTFとVehicle Modelを設定

Map Tabでblamから生成した地図とTFを設定

Sensing Tabでvoxel_grid_filterを選択

Computing Tab

ndt_matchingのappでInitial Posを選択

ndt_matching、vel_pose_connect、waypoint_saverを選択

Rosbag再生開始

Rosbag再生完了、経路生成結果