シミュレーションによる自律移動ロボット（AMR）のマッピングとナビゲーション

マッピングでは、自律移動ロボットを動かして、地図を作成します。
ナビゲーションでは、自律移動ロボットが、作成した地図に基づき、自己位置を認識し、目標位置への経路を計画し移動します。

自律移動ロボット（AMR）のマッピング

シミュレーションによる自律移動ロボットの地図作成の様子

上の画像は、シミュレータGazeboの画面（左）と、RViz2の画面（右）

マッピングでは、自律移動ロボット（AMR）を移動させて、部屋の地図を作成します。

シミュレータ上で地図作成の準備

シミュレータGazeboを起動します。willowgarageの部屋を使います。

ros2 launch hajimecart_gazebo willowgarage_world.launch.py

willowgarage_world.launch.py

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command, LaunchConfiguration


def generate_launch_description():

  pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
  pkg_my_gazebo = get_package_share_directory('hajimecart_gazebo')

  worlds = os.path.join(pkg_my_gazebo, 'worlds', 'willowgarage.world')

  urdf_model_dir = get_package_share_directory('hajimecart_description')
  urdf_model_path = os.path.join(urdf_model_dir, 'urdf', 'hajimecart_gazebo.urdf')

  model_arg = DeclareLaunchArgument(name='model', default_value=str(urdf_model_path),
                    description='Absolute path to robot urdf file')

  robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]),
                     value_type=str)

  # Gazebo launch
  gazebo = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
      os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'),
    )
  )

  spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
            arguments=['-entity', 'hajimecart', '-topic', '/robot_description',
            '-x', '8',
            '-y', '-8',
            '-z', '0',
            '-Y', '3.141592654'],
            output='screen')

  robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    parameters=[{'robot_description': robot_description}]
  )

  return LaunchDescription([
    DeclareLaunchArgument(
      'world',
      default_value=[worlds, ''],
      description='SDF world file'),
    model_arg,
    gazebo,
    spawn_entity,
    robot_state_publisher
  ])

地図作成プログラムを起動します。

ros2 launch hajimecart_navigation online_async_launch.py use_sim_time:=True

online_async_launch.py

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
  use_sim_time = LaunchConfiguration('use_sim_time')
  slam_params_file = LaunchConfiguration('slam_params_file')
  default_params_file = os.path.join(get_package_share_directory("hajimecart_navigation"),
                     'config', 'mapper_params_online_async.yaml')

  declare_use_sim_time_argument = DeclareLaunchArgument(
    'use_sim_time',
    default_value='true',
    description='Use simulation/Gazebo clock')
  declare_slam_params_file_cmd = DeclareLaunchArgument(
    'slam_params_file',
    default_value=default_params_file,
    description='Full path to the ROS2 parameters file to use for the slam_toolbox node')

  start_async_slam_toolbox_node = Node(
    parameters=[
      slam_params_file,
      {'use_sim_time': use_sim_time}
    ],
    package='slam_toolbox',
    executable='async_slam_toolbox_node',
    name='slam_toolbox',
    output='screen')

  ld = LaunchDescription()

  ld.add_action(declare_use_sim_time_argument)
  ld.add_action(declare_slam_params_file_cmd)
  ld.add_action(start_async_slam_toolbox_node)

  return ld

willowgarage.world

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://willowgarage</uri>
    </include>
  </world>
</sdf>

地図作成の様子を画面に表示するために、RViz2を起動します。

ros2 launch hajimecart_description display_nav.launch.py use_sim_time:=True

シミュレータ上で地図作成

キーボードで自律移動ロボットを操縦して、部屋を周ります。

ros2 run teleop_twist_keyboard teleop_twist_keyboard

RViz2の画面上に生成されている地図が表示されます。

自律移動ロボットが動いていくと地図が生成されていきます。画面を見ながら地図の出来具合を確認します。

作成した地図を保存

地図がよい具合に出来上がったら、地図を保存します。この例では、ファイル名 my_map-willowに地図を出力します。

ros2 run nav2_map_server map_saver_cli -f my_map-willow

作成した地図です。

作成した地図

画像で、黒線が占有領域（壁や障害物）、白色が自由領域（移動可能）、グレーが未知領域です。

動画

地図作成の様子を動画にしました。

シミュレーションによる自律移動ロボットの地図作成 (ROS2 Humble)

自律移動ロボット（AMR）のナビゲーション

シミュレーションによる自律移動ロボットのナビゲーションの様子

上の画像は、シミュレータGazeboの画面（左）と、RViz2の画面（右）

ナビゲーションでは、自律移動ロボット（AMR）が、自己位置を認識し、目標位置への経路を計画し移動します。

シミュレータを起動

シミュレータGazeboを起動します。地図作成に使用したwillowgarageの部屋を使います。

ros2 launch hajimecart_gazebo willowgarage_world.launch.py

シミュレータでナビゲーションを起動

ナビゲーションプログラムを起動します。地図は上で作成した地図を指定します。

ros2 launch hajimecart_navigation navigation2.launch.py use_sim_time:=True map:=${HOME}/hajimecart_ws/src/hajimecart_navigation/maps/my_map-willow.yaml

navigation2.launch.py

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    map_dir = LaunchConfiguration(
        'map',
        default=os.path.join(
            get_package_share_directory('hajimecart_navigation'),
            'maps',
            'my_map.yaml'))

    param_dir = LaunchConfiguration(
        'params_file',
        default=os.path.join(
            get_package_share_directory('hajimecart_navigation'),
            'params',
            'nav2_params.yaml'))

    nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')

    return LaunchDescription([
        DeclareLaunchArgument(
            'map',
            default_value=map_dir,
            description='Full path to map file to load'),

        DeclareLaunchArgument(
            'params_file',
            default_value=param_dir,
            description='Full path to param file to load'),

        DeclareLaunchArgument(
            'use_sim_time',
            default_value='True',
            description='Use simulation (Gazebo) clock if true'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
            launch_arguments={
                'map': map_dir,
                'use_sim_time': use_sim_time,
                'params_file': param_dir}.items(),
        )
    ])

画面表示および操作用にRViz2を起動します。

ros2 launch hajimecart_description display_nav.launch.py use_sim_time:=True

display_nav.launch.py

import os

from ament_index_python.packages import get_package_share_path
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command, LaunchConfiguration

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='True')

    urdf_model_dir = get_package_share_path('hajimecart_description')
    urdf_model_path = os.path.join(urdf_model_dir, 'urdf', 'hajimecart.urdf')

    model_arg = DeclareLaunchArgument(name='model', default_value=str(urdf_model_path),
                                      description='Absolute path to robot urdf file')
                                      
    rviz_config_dir = get_package_share_path('hajimecart_description')
    rviz_config_path = os.path.join(rviz_config_dir, 'rviz', 'nav2_default_view.rviz')
#    rviz_config_path = os.path.join(rviz_config_dir, 'rviz', 'slam_toolbox_default.rviz')

    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(rviz_config_path),
                                     description='Absolute path to rviz config file')

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]),
                                       value_type=str)
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_description}]
    )

    return LaunchDescription([
        model_arg,
        rviz_arg,
        rviz_node,
        robot_state_publisher_node
    ])

RViz2を起動すると、画面上に地図が現れます。

自律移動ロボットの現在位置と向きを設定

自律移動ロボットの現在位置と向きを設定します。
RViz2の上のメニューから「2D Pose Estimate」をクリックします。
表示された緑色の矢印で、矢印の起点がロボットの位置、矢印の先がロボットの向きになるように設定します。
設定したロボットの現在位置の周りに、緑色の点群が表示されます。

＜自己位置の推定＞
車輪の回転量から計算した位置（オドメトリ）と、LiDARの情報と地図を照らし合わせて、確率な方法で求めます。
地図の黒線の周囲が水色とピンク色で表示されます。

＜グローバル・コストマップ＞
地図にある障害物の情報を管理します。
ロボット近辺ではコストマップの色が濃くなります。

＜ローカル・コストマップ＞
センサ（LiDAR）による障害物の情報を管理します。

自律移動ロボットの目標位置と向きを設定

目標地点を設定します。
RViz2の上のメニューから「Nav2 Goal」をクリックします。
表示された緑色の矢印で、ロボットの目標位置と向きを設定します。

現在位置と目標位置の間に、赤い線が引かれます。

＜グローバル・パス（経路）＞
地図に基づき、障害物を避けて（グローバル・コストマップ）、目標位置までの経路です。
ロボットはおおむね赤い線に沿って進みます。

＜ローカル・パス（経路）＞
円弧状の青い線はグローバルの経路に沿うように、障害物を避けて（ローカル・コストマップ）、ロボットの加速度を考慮した経路です。

自律移動ロボットの移動

径路が計算されると、自律移動ロボットは移動を始めます。
ロボットには、移動機構による移動方向の制限、速度や加速度の制限があります。それらを考慮し、さらに、LiDARにより検出された地図にない新たな障害物を避けて進みます。
目標位置と向きに到達するとロボットは停止します。

動画

自律移動ロボットでナビゲーションにより移動するシミュレーション（ROS2 Humble使用）