ROS2 Tugbot Gazebo で 3D LiDAR の SLAM を試してみた。

3. 手直し。
1) LeGO-LOAM/config/loam_config.yaml の修正
下記のように、一部修正しました。
# VLP-16 image_projection: ros__parameters: laser: num_vertical_scans: 16 #num_horizontal_scans: 1800 # changed by nishi 900 だった気がする。 # src/tugbot_my/tugbot_gazebo_my/models/tugbot.sdf # <sensor name='scan_omni' type='gpu_lidar'> num_horizontal_scans: 900 .....

2) LeGO-LOAM/CMakeLists.txt を一部修正。
jazzy 対応が入っていなかったので、入れました。
... message(STATUS "${LIB_NAME} Ros version : $ENV{ROS_DISTRO}") if ($ENV{ROS_DISTRO} MATCHES "iron") add_definitions(-DUSE_IRON_VERSION) # add by nishi 2025.10.12 elseif ($ENV{ROS_DISTRO} MATCHES "jazzy") #add_definitions(-DUSE_JAZZY_VERSION) add_definitions(-DUSE_IRON_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "humble") add_definitions(-DUSE_HUMBLE_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "galactic") add_definitions(-DUSE_GALACTIC_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "foxy") add_definitions(-DUSE_FOXY_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "eloquent") add_definitions(-DUSE_ELOQUENT_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "dashing") add_definitions(-DUSE_DASHING_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "noetic") add_definitions(-DUSE_ROS_NORTIC_VERSION) elseif ($ENV{ROS_DISTRO} MATCHES "melodic") add_definitions(-DUSE_ROS_MELODIC_VERSION) endif() ....

3) run.launch.py を自分用にアレンジ。
LeGO-LOAM/launch/run_nishi.launch.py を自分用に作成しました。
#!/usr/bin/env python3 # # src/LeGO-LOAM-ROS2/LeGO-LOAM/launch/run_nishi.launch.py # # 1. build # $ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select lego_loam_sr # $ . install/setup.bash # # 2. run # # term1 # $ sudo ufw disable # $ ros2 launch tugbot_gazebo_my warehouse.launch.py # # term2 # $ ros2 launch lego_loam_sr run_nishi.launch.py RVIZ:=False # # term3 # $ ros2 launch lego_loam_sr run_nishi.launch.py RVIZ:=True # # 3. control # 1) Teleop keyboard # $ ros2 run turtlebot3_teleop teleop_keyboard import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import SetEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from os.path import expanduser from launch.conditions import IfCondition, UnlessCondition def generate_launch_description(): # Configure environment stdout_linebuf_envvar = SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') stdout_colorized_envvar = SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1') # Simulated time use_sim_time = LaunchConfiguration('use_sim_time', default='true') # Nodes Configurations config_file = os.path.join(get_package_share_directory('lego_loam_sr'), 'config', 'loam_config.yaml') rviz_config = os.path.join(get_package_share_directory('lego_loam_sr'), 'rviz', 'test.rviz') # add by nishi DeclareLaunchArgument('RVIZ',default_value='false', description='Launch SBC (optional).'), RVIZ = LaunchConfiguration('RVIZ') # Tf transformations transform_map = Node( package='tf2_ros', executable='static_transform_publisher', name='camera_init_to_map', arguments=['0', '0', '0', '1.570795', '0', '1.570795', 'map', 'camera_init'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), ) transform_camera = Node( package='tf2_ros', executable='static_transform_publisher', name='base_link_to_camera', #arguments=['0', '0', '0', '-1.570795', '-1.570795', '0', 'camera', 'base_link'], arguments=['0', '0', '0', '-1.570795', '-1.570795', '0', 'camera', 'base_footprint'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), ) transform_velodyne = Node( package='tf2_ros', executable='static_transform_publisher', name='velodyne_to_base_link', arguments=['0', '0', '0', '0', '0', '0','base_link','velodyne'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), ) # LeGO-LOAM lego_loam_node = Node( package='lego_loam_sr', executable='lego_loam_sr', output='screen', parameters=[config_file], #remappings=[('/lidar_points', '/velodyne_points')], remappings=[('/rslidar_points', '/scan_omni/scan/points')], condition=UnlessCondition(LaunchConfiguration('RVIZ')), ) # Rviz rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', arguments=['-d', rviz_config], output='screen', condition=IfCondition(LaunchConfiguration('RVIZ')), ) ld = LaunchDescription() # Set environment variables ld.add_action(stdout_linebuf_envvar) ld.add_action(stdout_colorized_envvar) # Add nodes ld.add_action(lego_loam_node) ld.add_action(transform_map) ld.add_action(transform_camera) #ld.add_action(transform_velodyne) ld.add_action(rviz_node) return ld

4. 正式に run

# term1
$ sudo ufw disable
$ ros2 launch tugbot_gazebo_my warehouse.launch.py

# term2
$ ros2 launch lego_loam_sr run_nishi.launch.py RVIZ:=False

# term3
$ ros2 launch lego_loam_sr run_nishi.launch.py RVIZ:=True

3. control
1) Teleop keyboard
$ ros2 run turtlebot3_teleop teleop_keyboard


これで、一応試せるようになった。
Teleop keyboard の操作で、Tugbot を動かしながら、Rviz2 の表示を確認する。
2D Map のようなものが、Rviz2 の Ground Plane に出てくる。
だが、障害物はよいのだが、床も同じように白くなるので、区別が出来ない。
Tugbot が移動するにつれて、最初の情報が消えて行く。
Local_cost_map みたいな感じか。
$ ros2 topic info --verbose /ground_cloud
ただし、map topic では無いので、自分で変換しないと、Map 作成にはならないのではないか?
現状、Gazebo の Tugbot の向きと Rviz2 の向き がすこしずつずれるみたい。
注) これは、動かしていると、周囲の 3D Lidar の情報からうまく自己位置を算出して、Gazebo 上の位置と、うまく一致するみたい。
これを、rtabmap_ros の様に使うにはどうすればよいのか?

いまは、YouTube の映像と異なるので、まあ、設定が不十分なんだろう。
YouTube の映像だと、スタート位置からのデータが残っていて、最後にクロージャされているみたい。
本来は、このようにならないといかんのかも。

兎にも角にも、tf のリンクが不明なので、ここをなんとかしないといかんのかも。
# Tf transformations transform_map = Node( package='tf2_ros', executable='static_transform_publisher', name='camera_init_to_map', arguments=['0', '0', '0', '1.570795', '0', '1.570795', 'map', 'camera_init'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), ) transform_camera = Node( package='tf2_ros', executable='static_transform_publisher', name='base_link_to_camera', arguments=['0', '0', '0', '-1.570795', '-1.570795', '0', 'camera', 'base_link'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), ) transform_velodyne = Node( package='tf2_ros', executable='static_transform_publisher', name='velodyne_to_base_link', arguments=['0', '0', '0', '0', '0', '0','base_link','velodyne'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), )

base_link が、バッティングしているからどうするか?
下記を、変更を、lego_loam_sr の時だけ一時的に行う。

i) src/tugbot_my/tugbot_gazebo_my/models/tugbot.sdf
Gazebo から、 tf-odom -> tf-base_footprint を出さないようにする。
<plugin name='ignition::gazebo::systems::DiffDrive' filename='ignition-gazebo-diff-drive-system'> <!-- wheels --> <left_joint>wheel_left_joint</left_joint> <right_joint>wheel_right_joint</right_joint> <!-- kinematics --> <wheel_separation>0.5605</wheel_separation> <wheel_radius>0.195</wheel_radius> <!-- add by nishi --> <topic>cmd_vel</topic> <odom_topic>odom</odom_topic> <frame_id>odom</frame_id> <!-- <tf_topic>/tf</tf_topic> --> <!-- lego_loam_sr の時、 /tf が、バッティングするので、 /tf_gazebo にする。 by nishi 2025.10.13 --> <tf_topic>/tf_gazebo</tf_topic> <!-- lego_loam_sr の時、 base_footprint を link しない。 by nishi 2025.10.13 --> <!-- <child_frame_id>base_footprint</child_frame_id> --> <odom_publisher_frequency>30</odom_publisher_frequency> </plugin>

ii) LeGO-LOAM/launch/run_nishi.launch.py を下記に変更。
transform_camera = Node( package='tf2_ros', executable='static_transform_publisher', name='base_link_to_camera', #arguments=['0', '0', '0', '-1.570795', '-1.570795', '0', 'camera', 'base_link'], arguments=['0', '0', '0', '-1.570795', '-1.570795', '0', 'camera', 'base_footprint'], condition=UnlessCondition(LaunchConfiguration('RVIZ')), )

tf-map -> tf-camera_int -> tf-camera -> tf-basefoot_print にする。

これで、YouTube で見られるのと同じ動きをするみたい。

Map Cliud(/laser_cloud_surround) がずっとのこるみたい。
これが、クロージャーするのかも知れない。
よく観察していると、倉庫の中の周囲を判定して、Gazebo の位置とうまく一致するみたい。
すごい!!