はじめに

ROS2 環境でLaserScanメッセージとPointCloud2を相互に変換するpointcloud_to_laserscanの使い方のメモ

環境

ROS2 Humble , Jazzy

インストール

sudo apt install ros-$ROS_DISTRO-pointcloud-to-laserscan

LaserScanをPointCloud2に変換する

laserscan_to_pointcloud_nodeを使用
例としてTurtleBot3 Simulation のLaserScan型の/scanトピックをPointCloud2に変換する。
適当にlaserscan_to_pointcloud_nodeを起動するだけのlaunchを作成
実機で試す場合はGazeboの代わりにそれぞれのLiDARドライバを起動してください。

remappingsで入出力トピック名を設定

            remappings=[('scan_in', '/scan'),
                        ('cloud',  '/cloud')],

laserscan_to_pointcloud.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='pointcloud_to_laserscan',
            executable='laserscan_to_pointcloud_node',
            name='laserscan_to_pointcloud',
            remappings=[('scan_in', '/scan'),
                        ('cloud',  '/cloud')],
            parameters=[{'target_frame': '', 
                         'transform_tolerance': 0.01, 
                         'use_sim_time': True}]
        ),
    ])

実行

TurtleBot3のGazeboシミュレーションを起動

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

laserscan_to_pointcloudを実行　

ros2 launch laserscan_to_pointcloud.launch.py

出力のPointCloud2を購読するNodeがいないとLaserScanのSubscriberが動かない仕様なので注意

rviz2でPointCloudを見る

PublishするPointCloudのQoS設定がSensorDataQoSになっているので、TopicのReliability PolicyをBest Effortにするとrviz2に点群が表示される。

PointCloud2をLaserScanに変換する

pointcloud_to_laserscan nodeを使用
動作確認にはGazeboのvelodyne_simulatorを使う

/velodyne_pointsの上下0.2mのpointcloudをLaserScanに変換

remappingsで入出力トピック名を設定

            remappings=[('cloud_in', '/velodyne_points'),
                        ('scan', '/scan')],

pointcloud_to_laserscan.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
            remappings=[('cloud_in', '/velodyne_points'),
                        ('scan', '/scan')],
            parameters=[{
                'target_frame': '',
                'transform_tolerance': 0.01,
                'min_height': -0.2,
                'max_height': 0.2,
                'angle_min': -3.1415,  # -M_PI/2
                'angle_max': 3.1415,  # M_PI/2
                'angle_increment': 0.0087,  # M_PI/360.0
                'scan_time': 0.3333,
                'range_min': 0.45,
                'range_max': 30.0,
                'use_inf': True,
                'inf_epsilon': 1.0
            }],
            name='pointcloud_to_laserscan'
        )
    ])

velodyne_simulatorの実行

ros2 launch velodyne_description example.launch.py

ros2 launch pointcloud_to_laserscan.launch.py

rviz2でLaserScanを見る

白い点が変換後の/scan

参考