Ros2 地图保存

首先我启动bringup_launch.py，并将slam设置为True，来到达建图的效果。

# Launch the ROS 2 Navigation Stack
    start_ros2_navigation_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, 'bringup_launch.py')),
        launch_arguments = {'namespace': namespace,
                            'use_namespace': use_namespace,
                            'slam': slam,
                            'map': map_yaml_file,
                            'use_sim_time': use_sim_time,
                            'params_file': params_file,
                            'autostart': autostart}.items())

ringup_launch.py中其实最终是调用了ros-foxy-slam-toolbox（我ros2用的foxy版本）来进行建图。

如该通过命令：

ros2 run nav2_map_server map_saver_cli -f 

进行保存地图会报错。

但是如果使用cartographer进行建图：

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True
用上面的命令，是可以保存的。

这里想解决的问题是，slam-toolbox建图的情况下，如何保存地图。

这里我们要写一个节点，来启动map_server.

完整代码可以参考：

https://github.com/songhuangong/MyRosTesthttps://github.com/songhuangong/MyRosTest

#!/usr/bin/env python3

# This is the launch file used for ROS Foxy and older. 
# Launch this file to save a map created in RViz.
# 建图完成后，还需输入命令：
# ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"



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

def generate_launch_description():

    # .................. Configurable Arguments .....................

    use_sim_time = True
    map_saver_params_file = 'map_saver_params.yaml'
    # ...............................................................


    pkg_dir = get_package_share_directory('launch_test')
    map_save_config = os.path.join(pkg_dir, 'params', map_saver_params_file)


    return LaunchDescription([

        DeclareLaunchArgument("use_sim_time", default_value=str(use_sim_time), description="Use simulation/Gazebo clock"),
        DeclareLaunchArgument("map_saver_params_file", default_value=map_save_config, description="Map Saver Configuration File"),

        Node(
            package='nav2_map_server',
            executable='map_saver_server',
            output='screen',
            emulate_tty=True,  
            parameters=[LaunchConfiguration('map_saver_params_file')]                  
        ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager',
            output='screen',
            emulate_tty=True,  
            parameters=[
                {'use_sim_time': LaunchConfiguration('use_sim_time')},
                {'autostart': True},
                {'node_names': ['map_saver']}]
        )

    ])

Nav2里有个生命周期的节点管理，所以这里除了启动map_saver_server节点，还需将其节点的状态切换成Active才行。节点启动之后，就发如下命令就可以保存当前建好的地图了：

ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"