ROS2 Nav2 导航地图的使用

1. Nav2的基本知识

Nav2是ROS2的ROS导航堆栈的继承者,它是机器人从a点安全移动到B点所需的工具集合。它包括的工具有：

Map Server : Load, serve, and store environment maps
AMCL : Localize the robot on the map
Nav2 Planner : Plan a path from A to B around obstacles
Nav2 Controller : Control the robot as it follows the path 
Nav2 Costmap 2D : Convert sensor data into an obstacle-aware representation of the world 
Nav2 Recoveries : Compute recovery behaviors in case of failure
Nav2 Lifecycle Manager: Manage the lifecycle of the servers 
Nav2 BT Server: And plugins to enable your own custom algorithms and behaviors

2.构建一个地图

准备工作：

在gazebo中启动turtlebot3： ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

2.1 Cartographer_ros

构建地图使用的是Cartographer_ros这个预构建的包，准确地配置这个包就可以使用建图了。

1. 启动cartographer_node：

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

2. 启动occupancy_grid_node：

        package='cartographer_ros',
        executable='occupancy_grid_node',
        output='screen',
        name='occupancy_grid_node',
        parameters=[{'use_sim_time': True}],
        arguments=['-resolution', '0.05', '-publish_period_sec', '1.0']

Cartographer使用LUA编程语言来指定其配置，在config文件夹中写一个lua文件：
include “map_builder.lua”
include “trajectory_builder.lua”

options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.min_range = 0.12
TRAJECTORY_BUILDER_2D.max_range = 3.5
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.0
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

-- POSE_GRAPH.optimize_every_n_nodes = 0

return options

这个文件对应的就是 configuration_basename。综上，通过运行下面的launch文件就可以启动建图工具来建图了:

ros2 launch cartographer_slam cartographer.launch.py

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

    def generate_launch_description():

        cartographer_config_dir = os.path.join(get_package_share_directory('cartographer_slam'), 'config')
        configuration_basename = 'cartographer.lua'

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

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

2.2 保存地图数据

打开rviz2，添加 Map 就可以看到下图所示的地图

运行键盘控制指令 ros2 run teleop_twist_keyboard teleop_twist_keyboard

让小车跑一圈，把地图构建起来 . 之后在config文件夹中用 ros2 run nav2_map_server map_saver_cli -f turtlebot_area

保存地图数据turtlebot_area.pgm 和turtlebot_area.yaml

2.3 使用地图数据

使用cartographer创建地图并使用map_saver保存后，创建的地图需要提供给其他导航应用程序，如定位或路径规划器。因此需要启动map_server来加载地图,

同时也需要启动 lifecycle_manager 来管理 nav2中节点的生命周期,如下所示:

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

def generate_launch_description():
    
    map_file = os.path.join(get_package_share_directory('map_server'), 'config', 'turtlebot_area.yaml')

    return LaunchDescription([
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True}, 
                        {'yaml_filename':map_file} 
                    ]),

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

关于lifecycle_manager 和 ros2_control中的mangager差不多，回头单独写一个

项目代码