M2 MacBookでSLAM Toolboxを動かす 1

M2 MacBook AirでSLAM Tool Boxやります。

前回の記事まででGazeboの仮想的な空間を作ることが出来るようになりましたので、cost mapを作成します。

M2 MacBookでGazeboを使う 1
M2 MacBookでGazeboを使う 2

環境。

M2 MacBook Air
VMware Fusion
Ubuntu Server for ARM
ROS2 Humble

はじめに

cartographerが長い間使われていましたが、今はSLAM Toolboxということなので使っていきます。
ROS2としてのdefaultがgmappingからSLAM Toolboxに置き換えられたとのことです。

install (不要の場合)

install済みか確認します。


$ apt list --installed | grep 'slam-toolbox'

install済みでした。


ros-humble-slam-toolbox/now 2.6.8-1jammy.20240831.083145 arm64 [installed,upgradable to: 2.6.8-1jammy.20241128.121836]

(ARM版でgit cloneしてcolcon buildを通すのは難しいかもしれません。ですが、問題解析に役立つためgit cloneするのは有効と思います。最新にしているけどceresのheaderが足りない。なんとかなるんだろうけど。M2でRosseta 2を使うくらいなら、もうIntel CPUのPCを購入して開発するか。)

install


$ sudo apt update
$ sudo apt install ros-humble-slam-toolbox

apt installだとlifelong_launch.pyが無い。なんでや。ARMだからか ?
build通すか。まだまだ実験段階なのかな。

LGPLか。

動作確認


$ ros2 launch slam_toolbox online_async_launch.py

自分の環境の場合、下記のlogで止まったままになります。これが正常です。mapを作成できます。
これが、you can make a map !!とか表示してくれてたら迷わないのですが。


Registering sensor: [Custom Described Lidar]

準備 1

turtlebot3の環境を使います。

turtlebot3は開発終了していてignitionには対応していないようです。
かつM2 MacBookの様にCPUがARMの場合はgazebo ignitionしか選択肢が無いようです。

turtlebot3をignitionに対応させている環境がgithubにありますので利用させていただきます。

navigation2_ignition_gazebo_turtlebot3


$ cd ~/ros/robot_ws/src
$ git clone https://github.com/Onicc/navigation2_ignition_gazebo_turtlebot3.git
$ cd navigation2_ignition_gazebo_turtlebot3
$ git branch -a

humbleやironのことを気にしなくていいとわかります。


$ mv turtlebot3 ..
$ cd ..
$ rm -rf navigation2_ignition_gazebo_turtlebot3

colcon buildすればintelでturtlebot3環境を作った場合と同様に動作させることができます。

準備 2


$ cd turtlebot3/launch
$ cp simulation.launch.py simulation.launch.no_nav2.py

simulation.launch.no_nav2.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, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node

TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']   # waffle

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    world_name = LaunchConfiguration('world_name', default='turtlebot3_world')

    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3'), 'launch')

    ign_resource_path = SetEnvironmentVariable(
        name='IGN_GAZEBO_RESOURCE_PATH',value=[
        os.path.join("/opt/ros/humble", "share"),
        ":" +
        os.path.join(get_package_share_directory('turtlebot3'), "models")])

    # Spawn robot
    ignition_spawn_entity = Node(
        package='ros_ign_gazebo',
        executable='create',
        output='screen',
        arguments=['-entity', TURTLEBOT3_MODEL,
                   '-name', TURTLEBOT3_MODEL,
                   '-file', PathJoinSubstitution([
                        get_package_share_directory('turtlebot3'),
                        "models", "turtlebot3", "model.sdf"]),
                   '-allow_renaming', 'true',
                   '-x', '-2.0',
                   '-y', '-0.5',
                   '-z', '0.01'],
        )

    # Spawn world
    ignition_spawn_world = Node(
        package='ros_ign_gazebo',
        executable='create',
        output='screen',
        arguments=['-file', PathJoinSubstitution([
                        get_package_share_directory('turtlebot3'),
                        "models", "worlds", "my_model.sdf"]),
                   '-allow_renaming', 'false'],
        )

    world_only = os.path.join(get_package_share_directory('turtlebot3'), "models", "worlds", "my_world_only.sdf")

    return LaunchDescription([
        ign_resource_path,
        ignition_spawn_entity,
        ignition_spawn_world,
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [os.path.join(get_package_share_directory('ros_ign_gazebo'),
                              'launch', 'ign_gazebo.launch.py')]),
            launch_arguments=[('ign_args', [' -r -v 3 ' +
                              world_only
                             ])]),

        DeclareLaunchArgument(
            'use_sim_time',
            default_value=use_sim_time,
            description='If true, use simulated clock'),

        DeclareLaunchArgument(
            'world_name',
            default_value=world_name,
            description='World name'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([launch_file_dir, '/ros_ign_bridge.launch.py']),
            launch_arguments={'use_sim_time': use_sim_time}.items(),
        ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([launch_file_dir, '/robot_state_publisher.launch.py']),
            launch_arguments={'use_sim_time': use_sim_time}.items(),
        ),

        # IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([launch_file_dir, '/navigation2.launch.py']),
        #     launch_arguments={'use_sim_time': use_sim_time}.items(),
        # ),
    ])

変更箇所は3箇所です。

        arguments=['-file', PathJoinSubstitution([
                        get_package_share_directory('turtlebot3'),
                        "models", "worlds", "my_model.sdf"]),
                   '-allow_renaming', 'false'],
        )

    # world_only = os.path.join(get_package_share_directory('turtlebot3'), "models", "worlds", "world_only.sdf")
    world_only = os.path.join(get_package_share_directory('turtlebot3'), "models", "worlds", "my_world_only.sdf")

        # IncludeLaunchDescription(
        #     PythonLaunchDescriptionSource([launch_file_dir, '/navigation2.launch.py']),
        #     launch_arguments={'use_sim_time': use_sim_time}.items(),
        # ),

準備 3

間違ってしまっても大丈夫なように元のfileは退避させておきます。
この時はやり過ぎなくらい退避させたい気分でした。


$ cd ~/ros/robot_ws/src/turtlebot3/models/worlds
$ cp model.sdf model.sdf.original
$ cp model.sdf my_model.sdf
$ cp world_only.sdf world_only.sdf.original
$ cp world_only.sdf my_world_only.sdf

my_model.sdf

<sdf version='1.5'>
</sdf>

my_world_only.sdf

<?xml version="1.0" ?>
<sdf version="1.9">
    <world name="Moving_robot">

        <physics name="1ms" type="ignored">
            <max_step_size>0.001</max_step_size>
            <real_time_factor>1.0</real_time_factor>
        </physics>

        <plugin
            filename="libignition-gazebo-physics-system.so"
            name="ignition::gazebo::systems::Physics">
        </plugin>

        <plugin
            filename="libignition-gazebo-user-commands-system.so"
            name="ignition::gazebo::systems::UserCommands">
        </plugin>

        <plugin
            filename="libignition-gazebo-scene-broadcaster-system.so"
            name="ignition::gazebo::systems::SceneBroadcaster">
        </plugin>

        <plugin
            filename="libignition-gazebo-sensors-system.so"
            name="ignition::gazebo::systems::Sensors">
            <render_engine>ogre2</render_engine>
        </plugin>

        <!--
        <gui fullscreen='false'>
            <plugin name='3D View' filename='MinimalScene'>
                <ignition-gui>
                    <title>3D View</title>
                    <property type='bool' key='showTitleBar'>false</property>
                    <property type='string' key='state'>docked</property>
                </ignition-gui>
                <engine>ogre2</engine>
                <scene>scene</scene>
                <ambient_light>0.4 0.4 0.4</ambient_light>
                <background_color>0.8 0.8 0.8</background_color>
                <camera_pose>-6 0 6 0 0.5 0</camera_pose>
            </plugin>

            <plugin name='Scene Manager' filename='GzSceneManager'>
                <ignition-gui>
                    <property key='resizable' type='bool'>true</property>
                    <property key='width' type='double'>5</property>
                    <property key='height' type='double'>5</property>
                    <property key='state' type='string'>floating</property>
                    <property key='showTitleBar' type='bool'>false</property>
                </ignition-gui>
            </plugin>

            <plugin name='Entity tree' filename='EntityTree'>
                <ignition-gui>
                    <property type='string' key='state'>docked</property>
                </ignition-gui>
            </plugin>

            <plugin name='Visualize Lidar' filename='VisualizeLidar'/>
        </gui>
        -->

        <light type="directional" name="sun">
            <cast_shadows>true</cast_shadows>
            <pose>0 0 10 0 0 0</pose>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
            <attenuation>
                <range>1000</range>
                <constant>0.9</constant>
                <linear>0.01</linear>
                <quadratic>0.001</quadratic>
            </attenuation>
            <direction>-0.5 0.1 -0.9</direction>
        </light>

        <model name="ground_plane">
            <static>true</static>
            <link name="link">
                <collision name="collision">
                <geometry>
                    <plane>
                    <normal>0 0 1</normal>
                    </plane>
                </geometry>
                </collision>
                <visual name="visual">
                <geometry>
                    <plane>
                    <normal>0 0 1</normal>
                    <size>100 100</size>
                    </plane>
                </geometry>
                <material>
                    <ambient>0.8 0.8 0.8 1</ambient>
                    <diffuse>0.8 0.8 0.8 1</diffuse>
                    <specular>0.8 0.8 0.8 1</specular>
                </material>
                </visual>
            </link>
        </model>

        <model name='wall_right'>
            <pose>0 -20 0 0 -0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>40 1 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>40 1 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='wall_top'>
            <pose>20 0 0 0 -0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>1 40 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>1 40 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='wall_left'>
            <pose>0 20 0 0 -0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>40 1 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>40 1 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='wall_bottom'>
            <pose>-20 0 0 0 -0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>1 40 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>1 40 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='obstacle_top_right'>
            <pose>9 -9 0 0 0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>2 2 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>2 2 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='obstacle_top_left'>
            <pose>9 9 0 0 0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>2 2 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>2 2 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='obstacle_bottom_right'>
            <pose>-8 -8 0 0 0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>4 4 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>4 4 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

        <model name='obstacle_bottom_left'>
            <pose>-8 8 0 0 0 0</pose>
            <link name='box_link'>
                <inertial>
                    <inertia>
                        <ixx>0.16666</ixx>
                        <ixy>0</ixy>
                        <ixz>0</ixz>
                        <iyy>0.16666</iyy>
                        <iyz>0</iyz>
                        <izz>0.16666</izz>
                    </inertia>
                    <mass>1</mass>
                    <pose>0 0 0 0 -0 0</pose>
                </inertial>
                <collision name='box_collision'>
                    <geometry>
                        <box>
                            <size>4 4 2.5</size>
                        </box>
                    </geometry>
                    <surface>
                        <friction>
                            <ode/>
                        </friction>
                        <bounce/>
                        <contact/>
                    </surface>
                </collision>
                <visual name='box_visual'>
                    <geometry>
                        <box>
                            <size>4 4 2.5</size>
                        </box>
                    </geometry>
                    <material>
                        <ambient>0.3 0.3 0.3 1</ambient>
                        <diffuse>0.7 0.7 0.7 1</diffuse>
                        <specular>1 1 1 1</specular>
                    </material>
                </visual>
                <pose>0 0 0 0 -0 0</pose>
                <enable_wind>false</enable_wind>
            </link>
            <static>false</static>
            <self_collide>false</self_collide>
        </model>

    </world>
</sdf>

準備 4

rviz2の準備をします。

その前にviのyamlの準備をします。

準備 5

rviz2を起動します。


$ rviz2

左下の[Add]をクリックします。
左下の[By topic]タブを選択します。

/mapの下のMapを選択します。[OK]をクリックします。

[File] >> [Save Config As]

起動scriptを配置してある、など使いやすいところに保存します。
勿論nav2やturtlebot3を参考にしてpackageの中にrvizフォルダーを作成してその下で管理しても良いです。

ここではcreate_a_map.rvizとするとします。

(準備 6)

今までの経験からcreate_a_map.rvizの編集が必要と想定していましたが、見やすい状態で一通り読んだら編集は不要とわかりましたので、準備6は不要です。

shell script

いつでもすぐにcost mapの作成を開始できるようにshell scriptを作成します。

create_a_map.sh

#! /bin/sh

gnome-terminal --geometry=80x20+50+10 -- bash -c ". /home/jn/.bashrc;
ros2 launch turtlebot3 simulation.launch.no_nav2.py"

gnome-terminal --geometry=40x20+550+10 -- bash -c ". /home/jn/.bashrc;
ros2 launch slam_toolbox online_async_launch.py"

gnome-terminal --geometry=80x20+550+10 -- bash -c ". /home/jn/.bashrc;
rviz2 -d create_a_map.rviz"

gnome-terminal --geometry=80x10+550+10 -- bash -c ". /home/jn/.bashrc;
ros2 run teleop_twist_keyboard teleop_twist_keyboard"

準備 7

上記の起動scriptを実行すると下記のようになります。

001

rvizのcost mapが小さく表示されているので大きく表示されるようにしてcreate_a_map.rvizを上書きします。

002

create a map 1

create a map 2

mapの保存。


$ ros2 run nav2_map_server map_saver_cli -f ~/my_map

003

成功 !!

まとめ

下記を使えば良いみたい。

navigation2/nav2_bringup/launch/slam_launch.py