以前、Raspberry Pi4 + ROS 1で広角カメラを使い、その画像のキャリブレーションまで行う手順（link）をまとめましたが、Raspberry Pi4 + ROS 2で同じことをしたので、防備録を兼ねて手順をまとめます。
ROS 2の環境構築はできているものとします。
デバイス、環境
Raspi / PC
- Raspberry Pi4 Model B
- Ubuntu Server 22.04
- ROS 2 humble
- RPi Camera (G), Fisheye Lens (広角カメラ)
- Desktop PC
- Ubuntu 22.04
- ROS 2 humble
自分はDesktop PCでrviz2を使っての動作確認とキャリブレーションを行いました。
Raspberry Piで表示とキャリブレーションを行うことも可能なので、必須ではありません。
広角カメラ
使用したカメラはwaveshareというメーカーの広角カメラ「RPi Camera (G), Fisheye Lens」です。
スイッチサイエンスで購入可能です。
スペック
- 静止画像の解像度: 2592 x 1944
- 最大フレームレート: 30 fps
- 対角画角: 160°
- 水平画角: 120°
通常のカメラの画角は62.2×48.8なので、倍以上の画角があります。
使い方
使い方はとても簡単で、通常のRaspi専用カメラと同じです。
フラットケーブルで接続した後、raspistillコマンドやraspividで画像、動画の撮影が可能です。
カメラ画像の表示
カメラからの画像の取得には、v4l2_cameraを使用しました。
インストール、ビルド
インストールはaptで可能ですが、自分の環境ではaptでインストールして実行すると以下のようなエラーがでてしまいました。（「height * step != size or」以降の数値は画像の高さ、幅によって変わります。）
[v4l2_camera_node-1] terminate called after throwing an instance of 'cv_bridge::Exception'
[v4l2_camera_node-1] what(): Image is wrongly formed: height * step != size or 1944 * 5184 != 10119168
[ERROR] [v4l2_camera_node-1]: process has died [pid 9414, exit code -6, cmd '/opt/ros/humble/lib/v4l2_camera/v4l2_camera_node --ros-args -r __node:=v4l2_camera_node -r __ns:=/v4l2_camera --params-file /tmp/launch_params_bfvkzl13'].
そのため、ソースコードを直接ビルドしました。
なおGithubリポジトリにはgalacticブランチはあるもののhumbleブランチはなかったためgalacticブランチを使用しました。
$ cd (ROSワークスペース)/src
$ git clone https://github.com/tier4/ros2_v4l2_camera.git
$ cd (ROSワークスペース)
$ colcon build --packages-select v4l2_camera
$ . install/setup.bash
実行
launchファイルを用意し、実行しました。
import launch
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
launch.actions.LogInfo(
msg="Camera."
),
# Node for USB Camera
Node(
package = 'v4l2_camera',
namespace = 'v4l2_camera',
executable = 'v4l2_camera_node',
name = 'v4l2_camera_node',
parameters = [{'image_size': [2592, 1944]}],
),
])
$ ros2 launch camera_laucnh.py
/v4l2_camera/image_rawというトピック名で出力されるはずなので、ros2 topic listでこのトピックがあること、また、rviz2デコのトピックを選択し、カメラ画像が表示されれば成功です。
圧縮画像の出力
圧縮した画像を出力することも可能です。その場合は以下をインストールしてください。
$ sudo apt install -y ros-humble-image-transport
$ sudo apt install -y ros-humble-image-transport-plugins
インストール後、上記と同じ手順で実行すると/v4l2_camera_mod/image_raw/compressedというトピックが増え、圧縮画像が出力されます。
キャリブレーション
このカメラは魚眼レンズが使われているので、上の画像のように画像が歪んでいるので、この歪みを補正します。
準備
キャリブレーションに使用するチェスボードを印刷し、板などに貼りつけます。
自分はこれを印刷しました。
キャリブレーションアプリのインストール
キャリブレーションを行うROSアプリをインストールします。
$ sudo apt install -y ros-humble-camera-calibration
キャリブレーション実行
v4l2_cameraを実行し、さらにキャリブレーションアプリを実行します。
$ ros2 run camera_calibration cameracalibrator --size 9x6 --square 0.025 image:=/v4l2_camera/image_raw camera:=/v4l2_camera
実行するとキャリブレーションウィンドウと、そこにカメラ画像が表示されます。カメラでチェスボードを写し、それが認識されると以下のような表示がされます。
X, Y, Size, Krewのそれぞれのゲージが緑になりCALIBRATEボタンが緑になって押下できる状態になるまで、位置や大きさなどを変えます。
CALIBRATEボタンを押すと、歪み補正用のパラメータの計算が行われ、完了したらSAVEボタンを押下します。
結果は/tmp/calibrationdata.tar.gzに出力されます。
キャリブレーション結果の編集
calibrationdata.tar.gzを展開するとost.yamlというファイルがあり、歪み補正用のパラメータが記載されています。
自分の環境ではこれをそのまま使用すると実行エラーとなったため、ファイル内のcamera_nameをデフォルトのnarrow_stereoからmmal_service_16.1に変更しました。
変更後の値はv4l2_cameraにカメラ名を出力するログを追加して確認しました。
V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
: rclcpp::Node{"v4l2_camera", options},
canceled_{false}
{
...(略)...
cinfo_ = std::make_shared<camera_info_manager::CameraInfoManager>(this, camera_->getCameraName());
RCLCPP_INFO(get_logger(), "camera name : %s\n", camera_->getCameraName().c_str()); // <-- 追加したログ出力
歪み補正
歪み補正アプリのインストール
$ sudo apt install -y ros-humble-image-pipeline
このimage_pipelineに含まれるimage_procを使用します。
launchファイルの準備
image_procを実行するlaunchファイルを用意します。
このlaunchファイルは、Githubリポジトリにあるサンプル（Link）をもとにして作成しました。
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import LaunchConfigurationEquals
from launch.conditions import LaunchConfigurationNotEquals
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
def generate_launch_description():
arg_namespace = DeclareLaunchArgument(
name='namespace', default_value='',
description=('namespace for all components loaded')
)
composable_nodes = [
ComposableNode(
package='image_proc',
plugin='image_proc::RectifyNode',
name='rectify_mono_node',
namespace=LaunchConfiguration('namespace'),
# Remap subscribers and publishers
remappings=[
('image', '/v4l2_camera/image_raw'),
('camera_info', '/v4l2_camera/camera_info')
],
)
]
arg_container = DeclareLaunchArgument(
name='container', default_value='',
description=(
'Name of an existing node container to load launched nodes into. '
'If unset, a new container will be created.'
)
)
# If an existing container is not provided, start a container and load nodes into it
image_processing_container = ComposableNodeContainer(
condition=LaunchConfigurationEquals('container', ''),
name='image_proc_container',
namespace=LaunchConfiguration('namespace'),
package='rclcpp_components',
executable='component_container',
composable_node_descriptions=composable_nodes,
output='screen'
)
# If an existing container name is provided, load composable nodes into it
# This will block until a container with the provided name is available and nodes are loaded
load_composable_nodes = LoadComposableNodes(
condition=LaunchConfigurationNotEquals('container', ''),
composable_node_descriptions=composable_nodes,
target_container=LaunchConfiguration('container'),
)
return LaunchDescription([
arg_namespace,
arg_container,
image_processing_container,
load_composable_nodes,
])
次にv4l2_cameraの実行時に歪み補正ファイルを読み込むようにlaunchファイルを変更します。なお、以下の例ではost.yamlをcamera_calib.yamlにリネームし、launchファイルと同じ階層に置いています。
import launch
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
camera_file = launch.substitutions.LaunchConfiguration(
'params', default=['file://', launch.substitutions.ThisLaunchFileDir(), '/camera_calib.yaml'])
return LaunchDescription([
launch.actions.LogInfo(
msg="Camera."
),
# Node for USB Camera
Node(
package = 'v4l2_camera',
namespace = 'v4l2_camera',
executable = 'v4l2_camera_node',
name = 'v4l2_camera_node',
parameters = [{'image_size': [2592, 1944], 'camera_info_url': camera_file}],
),
])
歪み補正の実行
2つのlaunchファイルを実行します。
$ ros2 launch launch/camera_launch.py
$ ros2 launch launch/image_rect.py
補正後の画像は/image_rectというトピック名で出力されます。表示してみると歪みが補正されていることがわかります。
上が補正前の画像、下が補正後の画像です。
（この画像では補正しきれず一部歪んでいますが、何度かキャリブレーションを試したらいい感じに補正できるパラメータを出力できました。）
参考： 画像サイズによる遅延時間
Raspberry Pi4でカメラ画像の取得と歪み補正を行った場合にどれくらい遅延が発生するかを、カメラ画像のサイズを
- 最大サイズ（2592x1944）、
- 1/2(1296, 972)
- 1/3(864, 648)
- 1/4(648, 486)
と変更し確認しました。
不要な負荷を無くすため、rviz2はPCで実行しました。また、Raspberry Pi4とPCの間は有線LANで接続しています。
遅延はタイマーをカメラで撮影しビューアに表示された時間とどれくらい差があるかで測定したので、あまり正確ではありませんが参考にはなると思います。
| 画像サイズ | 歪み補正 | 画像圧縮 | 遅延(秒) |
|---|---|---|---|
| 2592 x 1944 | なし | なし | 2.6 |
| 1296 x 972 | なし | なし | 0.3 |
| 864 x 648 | なし | なし | 0.2 |
| 648 x 486 | なし | なし | 0.1 |
| 2592 x 1944 | あり | なし | 3.2 |
| 1296 x 972 | あり | なし | 0.3 |
| 864 x 648 | あり | なし | 0.3 |
| 648 x 486 | あり | なし | 0.1 |
| 2592 x 1944 | あり | あり | 4.2 |
| 1296 x 972 | あり | あり | 0.4 |
| 864 x 648 | あり | あり | 0.3 |
| 648 x 486 | あり | あり | 0.2 |
フルサイズ（2592 x 1944）だと、かなり遅延がありますが、1/2以下だと遅延は小さくなっていました。
1/2より小さい場合は歪み補正による遅延はそれほど大きくありませんでした。
また画像圧縮による効果は、有線LANを使っているためかほぼない、あるいは圧縮による負荷の方が影響ありそうに思えます。
自分はラジコンに載せて使用する予定なので、(648 x 486)でないと遅延が目立ってしまい使えない結果となってしまいました。