工学男子の日常

モノづくりが好きな男子の日記です。

【ROS2】CartographerとNavigation2でお手軽経路生成する話

Linux ROS2 WSL2 ロボット

これの続きです。
kogakudanshi.hatenablog.jp

$ sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

launchファイルを編集

前回の記事でつくった~/ros2_ws/src/bringup/launch/bringup.launch.pyを書き換えます。

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution

def generate_launch_description():
    
    ldlidar_launch = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource([
            get_package_share_directory('ldlidar_node'),
            '/launch/ldlidar_with_mgr.launch.py'
        ])
    )

    laser_filter_node = Node(
            package="laser_filters",
            executable="scan_to_scan_filter_chain",
            parameters=[
                    PathJoinSubstitution([
                    get_package_share_directory("laser_filters"),
                    "examples", "box_filter_example.yaml",
            ])],
            remappings=[('/scan','/ldlidar_node/scan'),('/scan_filtered','/scan')],
    )

    tf2_base_link_ldlidar_base = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'ldlidar_base']
    )

    bringup_dir = get_package_share_directory('bringup')
    cartographer_config_dir = os.path.join(bringup_dir, 'config')
    configuration_basename = 'noimu_lds_2d.lua'

    use_sim_time = False
    resolution = '0.05'
    publish_period_sec = '1.0'

    cartographer_node = Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-configuration_directory', cartographer_config_dir,
                       '-configuration_basename', configuration_basename]
    )

    occupancy_grid_node = Node(
            package='cartographer_ros',
            executable='cartographer_occupancy_grid_node',
            name='cartographer_occupancy_grid_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-resolution', resolution,
                        '-publish_period_sec', publish_period_sec]
    )

    #navigation2の起動
    nav2_launch = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource([
            get_package_share_directory('nav2_bringup'),
            '/launch/navigation_launch.py'
        ]),
        launch_arguments={
                'use_sim_time': 'False',
        }.items()
    )

    rviz2_config = os.path.join(
        get_package_share_directory('bringup'),
        'rviz',
        'config.rviz'
    )

    rviz2_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=[["-d"], [rviz2_config]],
        remappings=[('/move_base_simple/goal','/goal_pose')] #目標位置姿勢を示すTopic
    )

    ld = LaunchDescription()
    ld.add_action(ldlidar_launch)
    ld.add_action(laser_filter_node)
    ld.add_action(tf2_base_link_ldlidar_base)
    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)
    ld.add_action(nav2_launch)
    ld.add_action(rviz2_node)
    return ld

Nav2のlaunch descriptionとrviz2のremappingを追加しました。

ビルドして実行

$ cd ~/ros2_ws
$ colcon build --symlink-install --packages-select bringup
$ source install/setup.bash
$ ros2 launch bringup bringup.launch.py

Rvizではまだ表示設定が完了していないので以下設定をします。

  1. Navigationグループにチェック→loacal及びglobalコストマップが表示される
  2. Add->By topicから/goal_poseを追加→Rvizの2D Goal Poseボタンで設定した位置姿勢が表示される
  3. Add->By topicから/planを追加→Nav2が生成した経路パスが表示される

設定したらCtrl+Sで保存しておきます。

2D Goal Poseボタンで目標位置姿勢を指示します。

Nav2で生成された経路

生成された経路が表示されました。

またロボットに渡される速度情報である/cmd_velトピックを見てみます。

$ ros2 topic echo /cmd_vel
linear:
  x: 0.26
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.1578947368421052
---
linear:
  x: 0.23263157894736847
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.1578947368421052
---
...

適切に出力されています。あとはこれをサブスクライブして適当な通信手段でマイコンに送信するノードを作れば（めんどい）、自律移動ロボットの完成です。もしくはros2_ontrolを使えば差動二輪、3輪オムニなどの形式ごとにモーター速度まで計算してくれるはずです（もっとめんどい）。


ちなみにIMUやOdometryなしのLiDAR一本足SLAMだと結構頻繁に位置推定が地平の彼方へと吹っ飛んでいきます。


あと現状コストマップが生成されていない(LiDARが届いていないなど)エリアは立入禁止エリアと認識されています。
qiita.com
起動直後だとコストマップが生成されていない箇所は結構あって不便ですので、こちらのサイトを参考にtrack_unknown_spaceとかをいい感じに設定するといい感じだと思います。


今回の記事だと簡略化のためにビヘイビアツリー(bt)やパラメータファイル、フレーム変換等は全く扱いませんでした。本格的に自律移動したい場合はそのへんをガリガリ記述していくことになるんだと思います。
記事の内容は以上になります。


ご覧いただきありがとうございました。