はじめに

前回の記事では,Cartographerでを使って地図を作成しました.今回は,その地図で自己位置推定を実装する方法を紹介します.

動作環境

項目	バージョン
ubuntu	22.04
ROS	ROS2 humble
Lidar	RPLidar A1 M8

↓RPLidar A1 M8

自己位置推定の手法

自己位置推定の手法として,EMCL（Extended Monte Carlo Localization）を用いました.
EMCLは,AMCL（Adaptive Monte Carlo Localization）を基盤に上田隆一先生が改良したパーティクルフィルタによる自己位置推定手法です.AMCLの課題として,ロボットが停止すると推定も停止する点や,C++とCで書かれたコードの複雑さが挙げられます.EMCLでは,ロボットの動作に関係なく一定周期で,tfからオドメトリデータを取得し,パーティクルの位置を更新.センサデータは非同期に取り込まれ,オドメトリ処理後に重みを更新してリサンプリングを行います.尤度場を用いて重みを更新し,重みの総和がしきい値を下回ると「膨張リセット」を実施します.このアプローチにより,ロボットが止まってもパーティクルが動作を続け,初期位置の微調整が行われます.ただし,実機での運用ではセンサーノイズやタイムスタンプ,tfの位置関係の管理に注意が必要です.

実装方法

マップの作成

マップの作成については,前回の記事を御覧ください.

emcl2のインストール

以下の手順に従って,ソースコードをクローンしてインストールしてください.

cd ~/ros2_ws/src
git clone https://github.com/Arcanain/emcl2.git

rvizファイルの作成

可視化用のrvizファイルを作成します.mapやロボット,ridarの点群が可視化できます.

Panels:
  - Class: rviz_common/Displays
    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /TF1
        - /TF1/Frames1
        - /TF1/Tree1
        - /LaserScan1
        - /PointCloud21
        - /Path1
        - /Path1/Offset1
        - /Marker1
        - /Marker2
        - /Marker2/Status1
        - /Marker2/Topic1
        - /Marker2/Namespaces1
      Splitter Ratio: 0.5833333134651184
    Tree Height: 550
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: nav2_rviz_plugins/Navigation 2
    Name: Navigation 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/RobotModel
      Collision Enabled: false
      Description File: ""
      Description Source: Topic
      Description Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /robot_description
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        base_footprint:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        drivewhl_l_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        drivewhl_r_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        dummy_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
        front_caster:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Mass Properties:
        Inertia: false
        Mass: false
      Name: RobotModel
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Class: rviz_default_plugins/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: false
        base_footprint:
          Value: false
        base_link:
          Value: true
        drivewhl_l_link:
          Value: false
        drivewhl_r_link:
          Value: false
        dummy_link:
          Value: false
        front_caster:
          Value: false
        laser_frame:
          Value: false
        map:
          Value: true
        odom:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        map:
          odom:
            base_link:
              base_footprint:
                {}
              drivewhl_l_link:
                {}
              drivewhl_r_link:
                {}
              front_caster:
                {}
              laser_frame:
                {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/LaserScan
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 47
      Min Color: 0; 0; 0
      Min Intensity: 47
      Name: LaserScan
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.05000000074505806
      Style: Flat Squares
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Best Effort
        Value: /scan
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/PointCloud2
      Color: 255; 255; 255
      Color Transformer: ""
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: Bumper Hit
      Position Transformer: ""
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.07999999821186066
      Style: Spheres
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Best Effort
        Value: /mobile_base/sensors/bumper_pointcloud
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/Map
      Color Scheme: map
      Draw Behind: true
      Enabled: true
      Name: Map
      Topic:
        Depth: 1
        Durability Policy: Transient Local
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /map
      Update Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /map_updates
      Use Timestamp: false
      Value: true
    - Alpha: 1
      Arrow Length: 0.019999999552965164
      Axes Length: 0.30000001192092896
      Axes Radius: 0.009999999776482582
      Class: rviz_default_plugins/PoseArray
      Color: 0; 180; 0
      Enabled: true
      Head Length: 0.07000000029802322
      Head Radius: 0.029999999329447746
      Name: Amcl Particle Swarm
      Shaft Length: 0.23000000417232513
      Shaft Radius: 0.009999999776482582
      Shape: Arrow (Flat)
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Best Effort
        Value: /particlecloud
      Value: true
    - Class: rviz_common/Group
      Displays:
        - Alpha: 0.30000001192092896
          Class: rviz_default_plugins/Map
          Color Scheme: costmap
          Draw Behind: false
          Enabled: true
          Name: Global Costmap
          Topic:
            Depth: 1
            Durability Policy: Transient Local
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/costmap
          Update Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/costmap_updates
          Use Timestamp: false
          Value: true
        - Alpha: 0.30000001192092896
          Class: rviz_default_plugins/Map
          Color Scheme: costmap
          Draw Behind: false
          Enabled: true
          Name: Downsampled Costmap
          Topic:
            Depth: 1
            Durability Policy: Transient Local
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /downsampled_costmap
          Update Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /downsampled_costmap_updates
          Use Timestamp: false
          Value: true
        - Alpha: 1
          Buffer Length: 1
          Class: rviz_default_plugins/Path
          Color: 255; 0; 0
          Enabled: true
          Head Diameter: 0.019999999552965164
          Head Length: 0.019999999552965164
          Length: 0.30000001192092896
          Line Style: Lines
          Line Width: 0.029999999329447746
          Name: Path
          Offset:
            X: 0
            Y: 0
            Z: 0
          Pose Color: 255; 85; 255
          Pose Style: Arrows
          Radius: 0.029999999329447746
          Shaft Diameter: 0.004999999888241291
          Shaft Length: 0.019999999552965164
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /plan
          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
            Max Value: 10
            Min Value: -10
            Value: true
          Axis: Z
          Channel Name: intensity
          Class: rviz_default_plugins/PointCloud
          Color: 125; 125; 125
          Color Transformer: FlatColor
          Decay Time: 0
          Enabled: true
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Max Intensity: 4096
          Min Color: 0; 0; 0
          Min Intensity: 0
          Name: VoxelGrid
          Position Transformer: XYZ
          Selectable: true
          Size (Pixels): 3
          Size (m): 0.05000000074505806
          Style: Boxes
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/voxel_marked_cloud
          Use Fixed Frame: true
          Use rainbow: true
          Value: true
        - Alpha: 1
          Class: rviz_default_plugins/Polygon
          Color: 25; 255; 0
          Enabled: false
          Name: Polygon
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/published_footprint
          Value: false
      Enabled: true
      Name: Global Planner
    - Class: rviz_common/Group
      Displays:
        - Alpha: 0.699999988079071
          Class: rviz_default_plugins/Map
          Color Scheme: costmap
          Draw Behind: false
          Enabled: true
          Name: Local Costmap
          Topic:
            Depth: 1
            Durability Policy: Transient Local
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/costmap
          Update Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/costmap_updates
          Use Timestamp: false
          Value: true
        - Alpha: 1
          Buffer Length: 1
          Class: rviz_default_plugins/Path
          Color: 0; 12; 255
          Enabled: true
          Head Diameter: 0.30000001192092896
          Head Length: 0.20000000298023224
          Length: 0.30000001192092896
          Line Style: Lines
          Line Width: 0.029999999329447746
          Name: Local Plan
          Offset:
            X: 0
            Y: 0
            Z: 0
          Pose Color: 255; 85; 255
          Pose Style: None
          Radius: 0.029999999329447746
          Shaft Diameter: 0.10000000149011612
          Shaft Length: 0.10000000149011612
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_plan
          Value: true
        - Class: rviz_default_plugins/MarkerArray
          Enabled: false
          Name: Trajectories
          Namespaces:
            {}
          Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /marker
          Value: false
        - Alpha: 1
          Class: rviz_default_plugins/Polygon
          Color: 25; 255; 0
          Enabled: true
          Name: Polygon
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/published_footprint
          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
            Max Value: 10
            Min Value: -10
            Value: true
          Axis: Z
          Channel Name: intensity
          Class: rviz_default_plugins/PointCloud
          Color: 255; 255; 255
          Color Transformer: RGB8
          Decay Time: 0
          Enabled: true
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Max Intensity: 4096
          Min Color: 0; 0; 0
          Min Intensity: 0
          Name: VoxelGrid
          Position Transformer: XYZ
          Selectable: true
          Size (Pixels): 3
          Size (m): 0.009999999776482582
          Style: Flat Squares
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/voxel_marked_cloud
          Use Fixed Frame: true
          Use rainbow: true
          Value: true
      Enabled: true
      Name: Controller
    - Class: rviz_common/Group
      Displays:
        - Class: rviz_default_plugins/Image
          Enabled: true
          Max Value: 1
          Median window: 5
          Min Value: 0
          Name: RealsenseCamera
          Normalize Range: true
          Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /intel_realsense_r200_depth/image_raw
          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
            Max Value: 10
            Min Value: -10
            Value: true
          Axis: Z
          Channel Name: intensity
          Class: rviz_default_plugins/PointCloud2
          Color: 255; 255; 255
          Color Transformer: RGB8
          Decay Time: 0
          Enabled: true
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Max Intensity: 4096
          Min Color: 0; 0; 0
          Min Intensity: 0
          Name: RealsenseDepthImage
          Position Transformer: XYZ
          Selectable: true
          Size (Pixels): 3
          Size (m): 0.009999999776482582
          Style: Flat Squares
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /intel_realsense_r200_depth/points
          Use Fixed Frame: true
          Use rainbow: true
          Value: true
      Enabled: false
      Name: Realsense
    - Class: rviz_default_plugins/MarkerArray
      Enabled: true
      Name: MarkerArray
      Namespaces:
        {}
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /waypoints
      Value: true
    - Alpha: 1
      Axes Length: 1
      Axes Radius: 0.10000000149011612
      Class: rviz_default_plugins/PoseWithCovariance
      Color: 255; 25; 0
      Covariance:
        Orientation:
          Alpha: 0.5
          Color: 255; 255; 127
          Color Style: Unique
          Frame: Local
          Offset: 1
          Scale: 1
          Value: true
        Position:
          Alpha: 0.30000001192092896
          Color: 204; 51; 204
          Scale: 1
          Value: true
        Value: true
      Enabled: true
      Head Length: 0.30000001192092896
      Head Radius: 0.10000000149011612
      Name: PoseWithCovariance
      Shaft Length: 1
      Shaft Radius: 0.05000000074505806
      Shape: Arrow
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /amcl_pose
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/PointCloud2
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 1
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: PointCloud2
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /particle_cloud
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 3
      Buffer Length: 5
      Class: rviz_default_plugins/Path
      Color: 0; 255; 0
      Enabled: true
      Head Diameter: 0.30000001192092896
      Head Length: 0.20000000298023224
      Length: 0.30000001192092896
      Line Style: Lines
      Line Width: 0.029999999329447746
      Name: Path
      Offset:
        X: 0
        Y: 0
        Z: 0
      Pose Color: 255; 85; 255
      Pose Style: None
      Radius: 0.029999999329447746
      Shaft Diameter: 0.10000000149011612
      Shaft Length: 0.10000000149011612
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /visualize_tgt_path
      Value: true
    - Class: rviz_default_plugins/Marker
      Enabled: true
      Name: Marker
      Namespaces:
        target_circle: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /look_ahead_range_marker
      Value: true
    - Class: rviz_default_plugins/Marker
      Enabled: true
      Name: Marker
      Namespaces:
        target_point: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /target_point_marker
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: map
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Covariance x: 0.25
      Covariance y: 0.25
      Covariance yaw: 0.06853891909122467
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /initialpose
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /clicked_point
    - Class: nav2_rviz_plugins/GoalTool
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Angle: -1.5799998044967651
      Class: rviz_default_plugins/TopDownOrtho
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Scale: 27.15713882446289
      Target Frame: base_link
      Value: TopDownOrtho (rviz_default_plugins)
      X: 0
      Y: 0
    Saved: ~
Window Geometry:
  Displays:
    collapsed: true
  Height: 704
  Hide Left Dock: true
  Hide Right Dock: true
  Navigation 2:
    collapsed: false
  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000264fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e00000264000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032000000015e000001440000014400fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f00000264fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000264000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002880000026400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  RealsenseCamera:
    collapsed: false
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 648
  X: 718
  Y: 27

launchファイルの作成

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node, SetParameter

from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    package_dir      = get_package_share_directory("emcl2")
    emcl_params_file = os.path.join(package_dir, "config", 'emcl2_params.yaml')
    map_file         = os.path.join(package_dir, "map", 'map.yaml')
    print('\nMAP',map_file)

    rviz = os.path.join(package_dir, "rviz" , "nav2_default_view.rviz")
    print('\nRVIZ2 PATH',rviz)


    lifecycle_nodes = ['map_server']
    

    return LaunchDescription([

        # map server node
        Node(
            package    = 'nav2_map_server',
            executable = 'map_server',
            name       = 'map_server',
            output     = 'screen',
            parameters = [{'yaml_filename': map_file, 
                           'use_sim_time' : False
                         }],
        ),

        # emcl node
        Node(
            package    = 'emcl2',
            name       = 'emcl2',
            executable = 'emcl2_node',
            output     = 'screen',
            parameters = [emcl_params_file],
        ),

        # static TF conversion (base_link -> laser_frame)
        Node(
            package    = 'tf2_ros',
            executable = 'static_transform_publisher',
            output     = 'screen',
            arguments  = ['0.0', '0.0', '0.2', '0.0', '0.0', '0.0', 'base_link', 'laser_frame'],
        ),

        # rviz node
        Node(
            package    = 'rviz2',
            executable = 'rviz2',
            name       = 'rviz2',
            arguments  = ['-d', rviz],
            parameters = [{'use_sim_time': False}],
            output     = 'screen'
        ),

        # life cycle node: manages the life cycle states of nodes
        Node(
            package    = 'nav2_lifecycle_manager',
            executable = 'lifecycle_manager',
            name       = 'lifecycle_manager_localization',
            output     = 'screen',
            parameters = [{'autostart': True},
                          {'node_names': lifecycle_nodes}]
        ),
    ])

このlaunchファイルは,次のノードを立ち上げています.
・map_server: 地図データを提供するノード.
・emcl2: 位置推定を行うEMCLノード.
・tf2_ros: base_linkとlaser_frameを位置関係を固定し,座標変換を提供するノード.
・rviz2: RVizによる可視化ツール.
・nav2_lifecycle_manager: ノードのライフサイクルを管理するノード.

# static TF conversion (base_link -> laser_frame)
Node(
    package    = 'tf2_ros',
    executable = 'static_transform_publisher',
    output     = 'screen',
    arguments  = ['0.0', '0.0', '0.2', '0.0', '0.0', '0.0', 'base_link', 'laser_frame'],
),

「laser_frame」の名前は,使うLiDARのフレームIDに適宜変更してください.

オドメトリの計算

emcl2は,tfからオドメトリの移動量を考慮して自己位置を推定しているので,オドメトリを計算する必要があります.高い精度で移動量を推定するには,モーターのエンコーダーからホイールオドメトリを算出しまする必要がありますが,以下のコードでは,簡易的に実装するため速度指令値(cmd_vel)を積算させてオドメトリの値を計算しています.

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"  // Pathを使用するために追加
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "tf2/LinearMath/Quaternion.h"  // tf2::Quaternionを使用するために追加
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_ros/transform_broadcaster.h"
#include "visualization_msgs/msg/marker.hpp"

using namespace std::chrono_literals;

class OdometryPublisher : public rclcpp::Node
{
public:
  OdometryPublisher()
  : Node("odometry_pub")
  {
    odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("odom", 50);
    path_pub = this->create_publisher<nav_msgs::msg::Path>("odom_path", 50);
    localmap_pub = this->create_publisher<nav_msgs::msg::OccupancyGrid>("local_map", 10);
    laser_range_pub = this->create_publisher<visualization_msgs::msg::Marker>(
      "laser_range_marker",
      10);
    odom_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(*this);

    // cmd_velサブスクライバを追加
    cmd_vel_subscriber = this->create_subscription<geometry_msgs::msg::Twist>(
      "cmd_vel", 10, std::bind(&OdometryPublisher::cmd_vel_callback, this, std::placeholders::_1));

    x = 0.0;
    y = 0.0;
    th = 0.0;

    current_time = this->get_clock()->now();
    last_time = this->get_clock()->now();

    path.header.frame_id = "odom";  // パスのフレームIDを設定

    timer_ = this->create_wall_timer(50ms, std::bind(&OdometryPublisher::timer_callback, this));

private:
  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
  {
    // cmd_velメッセージから速度を更新
    vx = msg->linear.x;
    vth = msg->angular.z;
  }

  void timer_callback()
  {
    /**
    *******************************************************************************************
    * Odometry Calculation
    *******************************************************************************************
    */
    current_time = this->get_clock()->now();

    double dt = (current_time - last_time).seconds();
    double delta_x = vx * cos(th) * dt;
    double delta_y = vx * sin(th) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

    tf2::Quaternion odom_quat;
    odom_quat.setRPY(0, 0, th);  // ロール,ピッチ,ヨーをセット
    geometry_msgs::msg::Quaternion odom_quat_msg =
      tf2::toMsg(odom_quat);  // tf2::Quaternionからgeometry_msgs::msg::Quaternionに変換

    geometry_msgs::msg::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat_msg;

    odom_broadcaster->sendTransform(odom_trans);

    nav_msgs::msg::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";

    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat_msg;

    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = vth;

    odom_pub->publish(odom);

    /**
    *******************************************************************************************
    * Odometry Path Publish
    *******************************************************************************************
    */
    // パスに現在の位置を追加
    geometry_msgs::msg::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = x;
    this_pose_stamped.pose.position.y = y;
    this_pose_stamped.pose.orientation = odom_quat_msg;
    this_pose_stamped.header.stamp = current_time;
    this_pose_stamped.header.frame_id = "odom";
    path.poses.push_back(this_pose_stamped);

    // パスを公開
    path_pub->publish(path);

    /**
    *******************************************************************************************
    * local costmap Publish
    *******************************************************************************************
    */
    // localmapの設定とpublish
    auto map = nav_msgs::msg::OccupancyGrid();
    map.header.stamp = current_time;
    map.header.frame_id = "base_link";
    map.info.resolution = 0.1;  // メートル/ピクセル
    map.info.width = 40;       // 4m x 4mの地図
    map.info.height = 40;
    map.info.origin.position.x = -(map.info.height * map.info.resolution) / 2.0;
    map.info.origin.position.y = -(map.info.width * map.info.resolution) / 2.0;
    map.info.origin.position.z = 0.0;
    map.info.origin.orientation.w = 1.0;

    // 地図データの初期化（すべてを未知(-1)に設定）
    map.data = std::vector<int8_t>(map.info.width * map.info.height, -1);

    // ローカルマップをパブリッシュ
    localmap_pub->publish(map);

    /**
    *******************************************************************************************
    * laser range visualize publish
    *******************************************************************************************
    */
    // 円の線を描くマーカーを作成
    visualization_msgs::msg::Marker line_strip_marker;
    line_strip_marker.header.frame_id = "base_link";
    line_strip_marker.header.stamp = current_time;
    line_strip_marker.ns = "circle_line";
    line_strip_marker.id = 1;
    line_strip_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
    line_strip_marker.action = visualization_msgs::msg::Marker::ADD;
    line_strip_marker.pose.orientation.w = 1.0;
    line_strip_marker.scale.x = 0.05;  // 線の太さ
    line_strip_marker.color.r = 1.0;
    line_strip_marker.color.g = 0.0;
    line_strip_marker.color.b = 0.0;
    line_strip_marker.color.a = 1.0;  // 不透明
    int num_points = 100;
    // 円周上の点を追加
    double radius = 1.0;
    for (int i = 0; i <= num_points; ++i) {
      double angle = i * 2.0 * M_PI / num_points;
      geometry_msgs::msg::Point p;
      p.x = radius * cos(angle);
      p.y = radius * sin(angle);
      p.z = 0.0;
      line_strip_marker.points.push_back(p);
    }

    // マーカーをパブリッシュth
    // 時刻の保存
    last_time = current_time;
  }

  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr localmap_pub;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr laser_range_pub;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber;
  std::shared_ptr<tf2_ros::TransformBroadcaster> odom_broadcaster;
  rclcpp::TimerBase::SharedPtr timer_;
  nav_msgs::msg::Path path;  // Pathメッセージのメンバ変数を追加
  double x, y, th, vx, vth;
  rclcpp::Time current_time, last_time;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<OdometryPublisher>());
  rclcpp::shutdown();
  return 0;
}

githubで公開しているのでインストールするか,ros2ファイルを新しく作成してください.

cd ~/ros2_ws/src
git clone https://github.com/Arcanain/arcanain_simulator.git

lidarを起動

source ~/ros2_ws/install/setup.bash
ros2 launch sllidar_ros2 sllidar_a1_launch.py

オドメトリの起動

source ~/ros2_ws/install/setup.bash
ros2 launch arcanain_simulator odometory_pub.py

emclを起動

source ~/ros2_ws/install/setup.bash
ros2 launch emcl2 emcl2_navigation_launch.py

emclを用いた経路追従制御

経路追従制御(pur pursuit法)の自己位置推定にemclを用いて実験しました.基本的には,精度良くマッチングしてくれましたが,初期座標,オドメトリの移動量,回転とセンサーによる位置推定にずれが生じると,自己位置が破綻しそうになる箇所がありました.EMCLは,適切なパラメータ設定と情報の組み合わせによって高精度な位置推定が可能です.しかし,自己位置推定の精度は,初期位置の設定,オドメトリの信頼性,センサーの精度,およびパーティクルの重み付けによって大きく左右されます.実験結果を基に,これらのパラメータを適切に調整し,オドメトリやセンサーの誤差が自己位置推定に与える影響を軽減することで,精度をさらに向上させることができそうです.

パラメータの設定

パラメータはLuaファイルで設定されていましたが,なぜかその設定が反映されていませんでした.そこで,直接コード内(src/emcl2_node.cpp)のパラメータを変更しました.ロボットの挙動に応じて,適宜調整してください.

// グローバル座標系の名前を設定します.通常は「map」として設定され,地図上の座標系.
this->declare_parameter("global_frame_id", std::string("map"));

// ロボットのフットプリント座標系の名前を設定します.通常は「base_footprint」で,ロボットの基準となる座標系.
this->declare_parameter("footprint_frame_id", std::string("base_footprint"));

// オドメトリの座標系の名前を設定します.通常は「odom」で,ロボットの移動履歴を追跡する座標系.
this->declare_parameter("odom_frame_id", std::string("odom"));

// ロボット本体の座標系の名前を設定します.通常は「base_link」で,ロボットの基準座標.
this->declare_parameter("base_frame_id", std::string("base_link"));

// オドメトリ情報の更新頻度　.
this->declare_parameter("odom_freq", 20);

// トランスフォームの許容誤差.これにより,座標系間の変換の信頼性が決まります.
this->declare_parameter("transform_tolerance", 0.2);

// レーザーセンサーの最小測定距離を設定します.0.0は測定距離の制限なしを意味します.
this->declare_parameter("laser_min_range", 0.0);

// レーザーセンサーの最大測定距離を設定します.
this->declare_parameter("laser_max_range", 100000000.0);

// スキャンの増分を設定します.センサーのスキャン解像度に関連します.
this->declare_parameter("scan_increment", 1);

// 初期位置のx座標を設定します.ロボットのスタート位置のx座標.
this->declare_parameter("initial_pose_x", 0.0);

// 初期位置のy座標を設定します.ロボットのスタート位置のy座標.
this->declare_parameter("initial_pose_y", 0.0);

// 初期位置の角度（ラジアン単位）を設定します.ロボットのスタート位置の向き.
this->declare_parameter("initial_pose_a", 0.0);

// EMCLで使用するパーティクルの数.
this->declare_parameter("num_particles", 500);

// パーティクルの重みの閾値.重みがこの閾値以下の場合,再サンプリングが行われます.
this->declare_parameter("alpha_threshold", 0.5);

// 位置の拡張半径.パーティクル群の位置更新の際に使用されます.
this->declare_parameter("expansion_radius_position", 0.1);

// 向きの拡張半径.パーティクル群の向き更新の際に使用されます.
this->declare_parameter("expansion_radius_orientation", 0.2);

// パーティクルの抽出率.再サンプリングの頻度を調整します.
this->declare_parameter("extraction_rate", 0.1);

// センサーの情報を元に位置推定の精度がどの程度低いとみなされるかを決定する範囲の閾値を設定します.
this->declare_parameter("range_threshold", 0.1);

// センサーのリセットを有効にするかどうか.デフォルトは無効です.
this->declare_parameter("sensor_reset", false);

// オドメトリの前進移動時の誤差.ロボットが前進した距離に応じた誤差です.
this->declare_parameter("odom_fw_dev_per_fw", 0.19);

// オドメトリの回転時の誤差.ロボットが回転した角度に応じた誤差です.
this->declare_parameter("odom_fw_dev_per_rot", 0.0001);

// オドメトリの回転時の誤差を前進移動量に基づいて設定します.
this->declare_parameter("odom_rot_dev_per_fw", 0.13);

// オドメトリの回転時の誤差を回転角度に基づいて設定します.
this->declare_parameter("odom_rot_dev_per_rot", 0.2);

// レーザーセンサーの信頼性を最大距離.この距離内でセンサーの情報が信頼されます.
this->declare_parameter("laser_likelihood_max_dist", 0.2);

参考文献

・上田隆一(2019), 詳解確率ロボティクス Pythonによる基礎アルゴリズムの実装

謝辞

この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.