velodyne公式ドライバ
velodyne公式ドライバで,velodyne VLP-16の点群を見るとチカチカすることが知られています.
nebulaを使うことでこれを回避することもできますが,公式ドライバでもチカチカしない方法を発見したので共有します.nebulaの入れ方はこちらからどうぞ
PC環境
項目 | 環境 |
---|---|
CPU | Intel Core Ultra5 125H |
RAM | 16GB |
OS | Ubuntu22.04 |
ROS 2 | Humble |
ドライバのインストール
git clone https://github.com/ros-drivers/velodyne.git -b ros2
git clone https://github.com/ros/angles.git -b ros2
git clone https://github.com/ros/diagnostics.git -b ros2-humble
cd ~/ros2_ws/
colcon build --symlink-install
velodyneのPC接続
PCI ethernet(LANケーブル)でもUSB ethernetでも動作確認済みです.
設定を開いて,+ボタンからアドレスを設定します.
IPv4にして,アドレスは192.168.1.100,ネットマスクは255.255.255.0と設定してください.
(ちなみにvelodyneのデフォルトのアドレスは192.168.1.201です.この値は使わないでください.)
ドライバの起動
ros2 launch velodyne velodyne-all-nodes-VLP16-launch.py
rviz2
addからby topicを選択して,pointcloud2を選択して,pointcloud2のタブを作りましょう.
rviz2では,fixed flameをvelodyneにしましょう
動画のようにチカチカして見えると思います.(逆にチカチカしてない場合は,これ以上の設定変更は不要です.)
この原因は,velodyne本体の回転周期とドライバの周期があっていないからです.詳しく説明すると,velodyneの周期が10Hz,ドライバの周期が20Hzで,velodyneが0.5回回転するうちにすでに点群が1回送信されてしまい,半回転の点群が見え,それが高速で切り替わっている状態です.
rviz2のflame rateを30Hzから10Hzにすると,チカチカしているのは,交互に見える場所が切り替わっていることがわかります.
実際
ros2 topic hz /velodyne_packets
average rate: 19.818
min: 0.050s max: 0.051s std dev: 0.00024s window: 21
average rate: 19.822
min: 0.050s max: 0.051s std dev: 0.00023s window: 41
average rate: 19.826
min: 0.050s max: 0.051s std dev: 0.00025s window: 61
ドライバでは20Hzとなっていることがわかると思います
対処法
対処法は簡単で,velodyne/velodyne_driver/config/VLP16-velodyne_driver_node-params.yamlの中のrpmを600から300に切り替えるだけです.
velodyne_driver_node:
ros__parameters:
device_ip: 192.168.1.201
gps_time: false
time_offset: 0.0
enabled: true
read_once: false
read_fast: false
repeat_delay: 0.0
frame_id: velodyne
model: VLP16
rpm: 300.0
port: 2368
timestamp_first_packet: false
もう一度起動しましょう.
ros2 launch velodyne velodyne-all-nodes-VLP16-launch.py
rviz2
チカチカしなくなりましたね.10Hzになっていることも確認できます.
ros2 topic hz /velodyne_packets
average rate: 9.980
min: 0.100s max: 0.101s std dev: 0.00024s window: 11
average rate: 9.981
min: 0.100s max: 0.101s std dev: 0.00024s window: 21
average rate: 9.981
min: 0.100s max: 0.101s std dev: 0.00024s window: 31
みなさんも良き点群ライフを!!
参考