RPLiDAR入門［3］〜点群の取得と可視化〜

今回は、RPLiDARを使って点群情報を取得し可視化してみようと思います。このシリーズの1回目ではRPLiDARの概要や種類について、2回目ではRPLiDARの仕組みについて説明しました。

専用のROSパッケージの導入

まず最初に、RPLiDARからROSを使って点群を取得、可視化してみます。専用のROSパッケージはSLAMTECが公開しているのでそれを使用します。パッケージ名はrplidar_rosです。

rplidar_rosを以下のコマンドを実行して導入してください。

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

ROSをインストールしていない方は、ROSをインストールするか、Docker経由でROSのデスクトップ環境を手に入れてください。私の場合は、ROS（noetic）がインストール済みのUbuntuのPCを使用しているので、そのまま実行しました。

Dockerを用いたROSデスクトップ環境の導入方法は、以下の記事の「DockerでROSのデスクトップ環境の作成」をご参考ください。

Rvizで可視化する

RPLiDARにはいくつかの種類があるので、所有しているRPLiDARに合わせてプログラムを実行します。RPLiDARごとのroslaunchは以下の通り。

今回、私が使用するのはA2なので、表の1行目のroslaunchを実行します。

cd ~/catkin_ws
source devel/setup.bash  # 必要があれば

roslaunch rplidar_ros view_rplidar.launch

実行すると、Rvizが起動し、取得された点群が表示されます。

以下の動画は、Rvizで点群を取得しながら、LiDARの回転を手で強制的に遅くしても点群がずれないことを確認したところです。

ノードとトピックの調査

以下のコードによりRvizを起動せずにRPLiDARを動かすことができます。

roslaunch rplidar_ros rplidar.launch

ここで、新しくターミナルを起動して、nodeやtopicの詳細を確認してみましょう。nodeを確認するときはrosnode list、トピックを確認するときはrostopic listを実行します。このとき、確認できた情報は下の表のとおりです。

/scanトピックのメッセージ型をrostopic type /scanで確認すると、sensor_msgs/LaserScanであることが確認できます。試しに、rostopic echo /scanを実行したときに得られたデータを以下に示します。

header: 
  seq: 6414
  stamp: 
    secs: 1663748061
    nsecs: 132936350
  frame_id: "laser"
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: 0.008278241381049156
time_increment: 0.00011606113548623398
scan_time: 0.08809040486812592
range_min: 0.15000000596046448
range_max: 12.0
ranges: [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.19599999487400055, 0.19599999487400055, 0.19599999487400055, inf, inf, inf, inf, 0.3009999990463257, 0.3009999990463257, 0.3009999990463257, 0.2980000078678131, 0.2980000078678131, 0.30000001192092896, 0.30000001192092896, 0.30000001192092896, 0.30300000309944153, 0.30300000309944153, 0.30300000309944153, inf, inf, inf, inf, 0.296999990940094, 0.296999990940094, 0.296999990940094, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.6150000095367432, 0.6150000095367432, 0.6150000095367432, 0.6200000047683716, 0.6200000047683716, inf, inf, inf, 0.20000000298023224, 0.20000000298023224, 0.20000000298023224, 0.19599999487400055, 0.19599999487400055, 0.19300000369548798, 0.19300000369548798, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.13099999725818634, 0.13099999725818634, 0.13099999725818634, 0.12999999523162842, 0.12999999523162842, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.13099999725818634, 0.13099999725818634, 0.13099999725818634, 0.13199999928474426, 0.13199999928474426, 0.13600000739097595, 0.13600000739097595, 0.16200000047683716, 0.16200000047683716, 0.16200000047683716, 0.16200000047683716, 0.16099999845027924, 0.16099999845027924, 0.16099999845027924, 0.16099999845027924, 0.16099999845027924, 0.16099999845027924, 0.16099999845027924, inf, inf, inf, inf, inf, inf, 0.21400000154972076, 0.21400000154972076, 0.21400000154972076, inf, inf, 0.21199999749660492, 0.21199999749660492, 0.21199999749660492, inf, inf, inf, inf, inf, inf, inf, 0.4970000088214874, 0.4970000088214874, 0.4970000088214874, inf, inf, inf, inf, 0.21899999678134918, 0.21899999678134918, 0.21899999678134918, inf, 0.2150000035762787, 0.2150000035762787, 0.2150000035762787, 0.21199999749660492, 0.21199999749660492, 0.210999995470047, 0.210999995470047, 0.210999995470047, 0.210999995470047, 0.20600000023841858, 0.20600000023841858, 0.20600000023841858, 0.2070000022649765, 0.2070000022649765, 0.20900000631809235, 0.20900000631809235, 0.20999999344348907, 0.20999999344348907, 0.20999999344348907, 0.210999995470047, 0.210999995470047, 0.210999995470047, 0.2150000035762787, 0.2150000035762787, 0.21699999272823334, 0.21699999272823334, 0.21699999272823334, 0.2199999988079071, 0.2199999988079071, inf, inf, inf, 0.20999999344348907, 0.20999999344348907, 0.20999999344348907, 0.21799999475479126, 0.21799999475479126, inf, inf, inf, 0.2199999988079071, 0.2199999988079071, 0.2199999988079071, inf, 0.22300000488758087, 0.22300000488758087, 0.22300000488758087, 0.22599999606609344, 0.1420000046491623, 0.1420000046491623, 0.14100000262260437, 0.14100000262260437, inf, inf, inf, inf, 0.24199999868869781, 0.24199999868869781, 0.24199999868869781, inf, 0.25, 0.25, 0.25, inf, 0.2540000081062317, 0.2540000081062317, 0.2540000081062317, inf, inf, inf, inf, inf, inf, inf, 0.17800000309944153, 0.17800000309944153, 0.17800000309944153, 0.17399999499320984, 0.17399999499320984, 0.18000000715255737, 0.18000000715255737, 0.17800000309944153, 0.17800000309944153, 0.17599999904632568, 0.17599999904632568, 0.1720000058412552, 0.1720000058412552, 0.16899999976158142, 0.16899999976158142, 0.16599999368190765, 0.16599999368190765, 0.16300000250339508, 0.16300000250339508, 0.16300000250339508, 0.16300000250339508, 0.16300000250339508, 0.1770000010728836, 0.1770000010728836, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.13600000739097595, 0.13600000739097595, 0.13600000739097595, 0.13500000536441803, 0.13500000536441803, 0.42100000381469727, 0.42100000381469727, 0.42100000381469727, inf, inf, inf, inf, inf, 0.382999986410141, 0.382999986410141, 0.382999986410141, 0.38199999928474426, 0.38199999928474426, 0.3930000066757202, 0.3930000066757202, inf, inf, inf, inf, inf, 0.24899999797344208, 0.24899999797344208, 0.24899999797344208, inf, inf, inf, 0.2549999952316284, 0.2549999952316284, 0.2549999952316284, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.15199999511241913, 0.15199999511241913, 0.15199999511241913, 0.15000000596046448, 0.15000000596046448, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 0.6050000190734863, 0.6050000190734863, 0.6050000190734863, 0.5870000123977661, 0.5870000123977661, 0.5809999704360962, 0.5809999704360962, 0.5820000171661377, 0.5820000171661377, 0.5830000042915344, 0.5830000042915344, 0.5830000042915344, 0.5879999995231628, 0.5879999995231628, inf, inf, inf, inf, 0.39500001072883606, 0.39500001072883606, 0.39500001072883606, 0.39500001072883606, 0.39500001072883606, 0.39500001072883606, 0.4020000100135803, 0.4020000100135803, 0.4020000100135803, inf, 0.3700000047683716, 0.3700000047683716, 0.3700000047683716, 0.367000013589859, 0.3709999918937683, 0.3709999918937683, 0.3709999918937683, 0.3869999945163727, 0.3869999945163727, inf, inf, inf, inf, inf, inf, 0.2029999941587448, 0.2029999941587448, 0.2029999941587448, 0.19599999487400055, 0.19599999487400055, 0.19599999487400055, 0.19099999964237213, 0.18799999356269836, 0.18799999356269836, 0.18700000643730164, 0.18700000643730164, 0.1860000044107437, 0.1860000044107437, 0.1860000044107437, 0.1860000044107437, 0.1860000044107437, 0.18700000643730164, 0.18700000643730164, 0.1860000044107437, 0.1860000044107437, 0.18199999630451202, 0.18199999630451202, 0.17900000512599945, 0.17900000512599945, 0.17800000309944153, 0.17800000309944153, 0.17800000309944153, 0.17800000309944153, 0.17800000309944153, 0.17800000309944153, 0.17800000309944153, 0.17900000512599945, 0.17900000512599945, 0.18000000715255737, 0.18000000715255737, 0.18299999833106995, 0.18299999833106995, 0.19200000166893005, 0.19200000166893005, 0.19699999690055847, 0.19699999690055847, inf, inf, inf, inf, 0.18199999630451202, 0.18199999630451202, 0.18199999630451202, 0.18000000715255737, 0.18000000715255737, 0.18000000715255737, 0.18000000715255737, 0.1809999942779541, 0.1809999942779541, 0.18199999630451202, 0.18199999630451202, 0.18199999630451202, 0.20200000703334808, inf, 0.22699999809265137, 0.22699999809265137, 0.22699999809265137, 0.2409999966621399, inf, inf, inf, 0.25999999046325684, 0.25999999046325684, 0.25999999046325684, inf, 0.2669999897480011, 0.2669999897480011, 0.2669999897480011, 0.2709999978542328, 0.27300000190734863, 0.27300000190734863, 0.27300000190734863, 0.27900001406669617, 0.28600001335144043, 0.28600001335144043, 0.28600001335144043, 0.2930000126361847, 0.2930000126361847, 0.2930000126361847, 0.3019999861717224, 0.3019999861717224, 0.3019999861717224, 0.3100000023841858, inf, 0.32100000977516174, 0.32100000977516174, 0.32100000977516174, 0.3319999873638153, 0.3319999873638153, 0.34200000762939453, 0.34200000762939453, 0.34200000762939453, 0.35199999809265137, 0.35199999809265137, 0.35199999809265137, 0.35199999809265137, 0.382999986410141, 0.382999986410141, 0.382999986410141, 0.37299999594688416, 0.37299999594688416, 0.39500001072883606, 0.39500001072883606, 0.39399999380111694, 0.39399999380111694, 0.39399999380111694, 0.4050000011920929, 0.4050000011920929, 0.4230000078678131, 0.4230000078678131, 0.4230000078678131, 0.4230000078678131, 0.44200000166893005, 0.44200000166893005, 0.44200000166893005, 0.44200000166893005, 0.44200000166893005, 0.44200000166893005, 0.4449999928474426, inf, inf, inf, inf, inf, inf, 5.0980000495910645, 5.0980000495910645, 5.0980000495910645, 5.052999973297119, 5.052999973297119, 5.320000171661377, 5.320000171661377, 5.359000205993652, 5.359000205993652, 5.359000205993652, 5.349999904632568, 5.349999904632568, 5.315000057220459, 5.315000057220459, 5.316999912261963, 5.316999912261963, 5.125, 5.125, 5.107999801635742, 5.107999801635742, 5.140999794006348, 5.140999794006348, 5.140999794006348, 5.169000148773193, 5.169000148773193, 5.160999774932861, 5.160999774932861, 5.127999782562256, 5.127999782562256, inf, 4.938000202178955, 4.938000202178955, 4.938000202178955, 4.9679999351501465, 4.9679999351501465, 4.9679999351501465, 3.885999917984009, 3.885999917984009, 3.4660000801086426, 3.4660000801086426, 3.2839999198913574, 3.2839999198913574, 3.0989999771118164, 3.0989999771118164, 2.9019999504089355, 2.9019999504089355, 2.799999952316284, 2.799999952316284, 2.7739999294281006, 2.7739999294281006, 2.515000104904175, 2.515000104904175, 2.503000020980835, 2.503000020980835, 2.503000020980835, 2.503000020980835, 2.503000020980835, 2.3389999866485596, 2.3389999866485596, 2.3499999046325684, 2.3499999046325684, 2.3510000705718994, 2.3510000705718994, 2.378999948501587, 2.378999948501587, 2.4700000286102295, 2.4700000286102295, 2.4700000286102295, 2.440999984741211, 2.440999984741211, 2.4769999980926514, 2.4769999980926514, 2.506999969482422, 2.506999969482422, 2.5, 2.5, 2.492000102996826, 2.492000102996826, 2.5260000228881836, 2.5260000228881836, 2.562999963760376, 2.562999963760376, 2.562999963760376, 2.6010000705718994, 2.6010000705718994, 2.638000011444092, 2.638000011444092, 2.6589999198913574, 2.6589999198913574, 2.7100000381469727, 2.7100000381469727, 2.753000020980835, 2.753000020980835, 2.7990000247955322, 2.7990000247955322, 2.7990000247955322, 2.8310000896453857, 2.8310000896453857, inf, inf, inf, inf, 1.309999942779541, 1.309999942779541, 1.309999942779541, 1.3179999589920044, 1.3179999589920044, 1.3179999589920044, 1.3240000009536743, 1.3240000009536743, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 1.1469999551773071, 1.1469999551773071, 1.1469999551773071, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.15000000596046448, 0.14900000393390656, 0.14900000393390656, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 1.5889999866485596, 1.5889999866485596, 1.5889999866485596]
intensities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0]
---

ここで注目していただきたいのは、sensor_msgs/LaserScan型のデータ構造についてと送られる数値に関してです。LaserScanは以下のような構造をしています。

header: 
  seq: 6414
  stamp: 
    secs: 1663748061
    nsecs: 132936350
  frame_id: "laser"
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: 0.008278241381049156
time_increment: 0.00011606113548623398
scan_time: 0.08809040486812592
range_min: 0.15000000596046448
range_max: 12.0
ranges: [・・・]
intensities: [・・・]
---

今回使用したRPLiDAR A2M8は、\(-\pi\)から\(+\pi\)の範囲（正確な範囲はこれより若干狭い）を0.008278241381049156[rad]刻みで計測することから、電卓で計算すると760点が得られることがわかります。ちなみに、A2M8の0度と角度の正方向の関係は以下の図のとおりです。RPLiDARを上から見たときに、コードの出ている付近が0[rad]（点のインデックスとしては379 or 380）で、配線付近から右回りに180度のあたりが\(-\pi\)[rad]（点のインデックスとしては0）、配線付近から左回りに180度のあたりが\(+\pi\)[rad]（点のインデックスとしては759）となります。

また、time_incrementに取得できる点数の760を乗算すると、scan_timeになります。

pythonで可視化してみよう

今回は、PythonでRPLiDARから出力された/scanトピックからリアルタイムで可視化するツールを作成してみます。

可視化であれば、Rvizでできるので、自分で作る必要性はないですが、ROSやPythonの勉強を兼ねてという感じですね。

今回は、lazerscan_vizというパッケージを作成します。

cd catkin_ws/src
catkin_create_pkg rplidar_pyviz rospy sensor_msgs

パッケージを作成したら、roscd rplidar_pyviz/srcを実行してフォルダを移動し、以下のコードをrplidar_pyviz.pyとして保存します。

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan 
import numpy as np
import matplotlib.pyplot as plt

def callback(msg):
    global num_points
    global x
    global y

    r = np.array(msg.ranges)

    for i in range(num_points):
        theta = msg.angle_min + msg.angle_increment * i
        x[i] = -r[i] * np.cos(theta)
        y[i] = -r[i] * np.sin(theta)

    x[-1] = -r[-1] * np.cos(msg.angle_max)
    y[-1] = -r[-1] * np.sin(msg.angle_max)

if __name__ == '__main__':
    rospy.init_node('rplidar_pyviz', anonymous=True)
    rospy.Subscriber("/scan", LaserScan, callback)

    global num_points
    global x
    global y

    num_points = 760
    x = np.zeros(num_points)
    y = np.zeros(num_points)

    fig, ax = plt.subplots()

    while not rospy.is_shutdown():
        ax.cla()
        ax.set_xlim(-15, 15)
        ax.set_ylim(-15, 15)
        ax.grid(True)
        ax.scatter(x, y)
        plt.draw()
        plt.pause(0.1)	

    rospy.spin()

保存したら、以下のコードを実行して、実行権限を与えます。

sudo chmod +x rplidar_pyviz.py

そうしたら、以下のコードを実行して、RPLiDARを起動して、今作ったプログラムを実行します。

roslaunch rplidar_ros rplidar.launch
rosrun rplidar_pyviz rplidar_pyviz.py

下図右に示したようにLiDARのコードが出ている方を表示された座標のx軸正方向に合わせて置くと、正しい向きで点群情報を可視化することができます。

さいごに

今回は、RPLiDARからROS経由でデータを取得して可視化してみました。RPLiDARシリーズは終了ですが、Autowareシリーズなどを実施していきたいと思います。