ros2 Navigation 学习笔记 第一、二章（the construct 网站）

第一章：ROS2 Navigation介绍

机器人如何导航

需要知道地点（建图）、环境中的位置（定位），计划如何在两点之间移动（路径规划），需要向轮子发送信息同时遵循路径（机器人控制和避障）

这些部分从头构建很复杂，因此可以使用ROS的导航包。

1.1 一个实例

进入工作空间（需要先建立一个）

cd ~/ros2_ws/src

下载例程到工作空间。代码是ros2 Galactic环境的

git clone https://bitbucket.org/theconstructcore/ros2_nav_files.git

编译并运行代码

source /opt/ros/galactic/setup.bash

cd ~/ros2_ws

colcon build

source install/setup.bash

ros2 launch nav2_course nav2_demo.launch.py

看到RVIZ的画面，点击右上角2D pose estimate，确定初始位置

点击Navigation2 Goal，给机器人赋目标

就完成了demo演示。

1.2 将要学习的内容

Nav2中的工具，包括Map Server, 定位机器人的工具AMCL, Nav2 Planner, Nav2 Controller, 传感器数据转换为世界系坐标Nav2 Costmap 2D, Nav2 Recoveries, Nav2 Lifecycle Manager, 自定义算法和行为插件Nav2 BT Sever

第二章 如何建图

简介

本章学习如何构建地图，涉及的主题包括

解释为什么需要地图、ROS2中需要什么建图，如何使用cartographer建图，如何将地图给ROS2其他部分，新的概念：Navigation lifecycle manager

什么是ROS中的地图

机器人使用地图来定位和计划轨迹。

ROS中使用占用网格地图（occupancy grid map）

需要什么来建图

一个有激光雷达和里程计的机器人。以及机器人环境。

SLAM

同时定位和建图。ROS2中可选cartographer或者slam-toolbox

cartographer更加简单易懂并且功能强大。

Cartographer_ros

Cartographer是谷歌开源的slam系统。

Cartographer_ros是Cartographer的ros软件包。

为机器人创建Cartographer的launch文件

为了启动cartographer，需要启动两个节点

1.启动cartographer_node

在节点启动中需要申明

cartographer_node是由cartographer_ros提供的

它需要的参数包括：

1、use_sim_time：是一个布尔值，指示节点是否必须将其时间与仿真同步

参数如下：

1、configuration_directory：查找配置文件的目录

2、configuration_basename：配置文件名

            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

需要在launch中指出的字段

occupancy_grid_node是由cartographer_ros提供的

需要的参数包括：

1、use_sim_time，同上

2、resolution：地图中每个栅格的米数

3、publish_period_sec：在map主题中发布的地图频率（以秒为单位）

            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.1

a)在工作空间中创建一个叫做cartographer_slam的新包

b)建立启动和配置目录

c)编写一个启动文件名为cartographer.launch.py，启动两个节点

注释

第一条

用以下行获取启动文件的配置目录

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

功能：将两条路径串联起来生成最后一条路径。此功能由OS提供因此需要导入。

get_package_share_directory是一个查找给定ROS包硬盘中完整路径的函数。此函数由ament_index_python.packages（您应该导入）提供。

因此需要import的内容是

import os
from ament_index_python.packages import get_package_share_directory

第二条

 在lunch文件中包含两个node的格式

    Node(
        ...
    ),
    
    Node(
        ...
    ),

在config目录中需要创建一个LUA文件cartographer.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

编译的时候需要注意 

打开cartographer_slam包的setup.py。加入

        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*')),

加入必须的imports

import os
from glob import glob

编译

cd ~/ros2_ws

colcon build --packages-select cartographer_slam

source ~/ros2_ws/install/setup.bash

接着执行launch文件

ros2 launch cartographer_slam cartographer.launch.py

在另一个终端执行

rviz2

在RVIZ中添加地图显示。点击ADD再点Map。在map display properities中设置主题为/map

添加TF和LaserScan。change the LaserScan -> Topic -> Reliability Policy from 'Reliable' to 'Best Effort' for the laser scan data to show.

将这个RVIZ初始化保存在src中命名为mapper_rviz_config.rviz.

再建立一个新的终端

ros2 run teleop_twist_keyboard teleop_twist_keyboard

就可以建图出相似的地图。

2.1的完整代码

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']
        ),
    ]) 

了解如何为不同的机器人配置cartographer

为了获得最佳效果，需要正确配置。所有配置都可以在lua文件中写

首先，cartographer自动订阅了如下话题

• /scan for laser data
• /odom for odometry data
• /imu for IMU data

重要提示：你不能为cartographer配置这些主题的值，因此如果你的机器人没有发布这些主题，就需要remapping。

以下是cartographer的集成顶级选项，所有这些配置必须在lua配置文件中指定。

1、一般参数

map_frame：发布子图的帧ID，poses的父帧，通常是map

tracking_frame：SLAM算法跟踪的帧的帧ID。如果用IMU，它应该处于其位置，尽管可能被旋转。常见的选择是base_link或者base_footprint。

published_frame：发布姿势的子帧的帧ID。如果odom帧由系统的不同部分提供，则为odom。在这种情况下，将发布map_frame中odom的资质，否则把它设置为“base_link”比较合适。

odom_frame：仅当provide_odom_frame为true的时候使用。published_frame和map_frame之间的帧将用于发布本地SLAM（非循环闭合）的结果。通常设为odom。

provide_odom_frame：如果启用，那么本地、非循环闭合、连续的姿势将发布为map_frame中的odom_frame。

use_odometry：如果启用，则订阅主题odom的nav_msgs/Odometry。在这种情况下必须提供里程计，信息将包含在SLAM中。

use_nav_sat：如果启用，则订阅主题fix上的sensor_msgs/NavSatFix。在这种情况下必须提供导航数据，信息将包含着SLAM全局中。

2、激光参数

num_laser_scans：要订阅的激光扫描主题的数量。订阅一台就在sensor_msgs/LaserScan的scan主题，如果多台就是scan_1,scan_2。

num_multi_echo_laser_scans：多回波激光扫描主题的数量，其余同上

num_subdivisions_per_laser_scan：要分割每个接受多回波激光扫描的点云数量。

num_point_clouds：订阅的点云主题数量。其余同12

3、滤波器参数

lookup_transform_timeout_sec：使用tf2查找转换时的超时。

submap_publish_period_sec：发布子贴图姿势的间隔，比如0.3秒

pose_publish_period_sec：发布姿势的间隔，比如0.005秒

trajectory_publish_period_sec：发布轨迹标记的时间间隔（秒）

odometry_sampling_ratio：里程计信息的固定比率采样

fixed_frame_sampling_ratio：固定帧消息的固定采样频率

imu_sampling_ratio：IMU消息的固定采样频率

landmarks_sampling_ratio：地标消息的固定采样频率

4、轨迹生成器参数

TRAJECTORY_BUILDER_2D.min_range：构建地图时将考虑的最小测量距离

TRAJECTORY_BUILDER_2D.max_range：构建地图时将考虑的最大测量距离

TRAJECTORY_BUILDER_2D.missing_data_ray_length：测量的距离作为丢失的激光

TRAJECTORY_BUILDER_2D.use_imu_data：是否使用imu

如何保存地图

要保存创建的地图需要运行map_saver节点，从nav2_map_server

重要提示：需要在保存地图的路径中调用节点

命令如下（在第四个终端中打开）

cd ~/ros2_ws/src/cartographer_slam/config

ros2 run nav2_map_server map_saver_cli -f turtlebot_area

预期的输出如下

[INFO] [1668679453.925832478] [map_saver]:
        map_saver lifecycle node launched.
        Waiting on external lifecycle transitions to activate
        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [1668679453.925942610] [map_saver]: Creating
[INFO] [1668679453.926403154] [map_saver]: Saving map from 'map' topic to 'turtlebot_area' file
[WARN] [1668679453.926451020] [map_saver]: Free threshold unspecified. Setting it to default value: 0.250000
[WARN] [1668679453.926488575] [map_saver]: Occupied threshold unspecified. Setting it to default value: 0.650000
[WARN] [map_io]: Image format unspecified. Setting it to: pgm
[INFO] [map_io]: Received a 160 X 141 map @ 0.05 m/pix
[INFO] [map_io]: Writing map occupancy data to turtlebot_area.pgm
[INFO] [map_io]: Writing map metadata to turtlebot_area.yaml
[INFO] [map_io]: Map saved
[INFO] [1668679454.042853506] [map_saver]: Map saved successfully
[INFO] [1668679454.042968106] [map_saver]: Destroying

重要提示：在调用map_saver节点之前不要关闭Cartographer节点，不然会丢失地图

会生成.pgm和.yaml两个文件

让地图被其他地方调用

一旦保存地图后，可以关闭终端。

创建的地图需要给其他导航应用程序，比如定位系统或者路径规划。为此需要启动map_server

你需要启动以下节点并且在RVIZ上可视化

map_server，nav2_lifecycle_manager

启动各个节点所需要的数据

1、对于启动map_server节点

该节点由nav2_map_server提供

需要的参数有：use_sim_time 布尔值，指示map_server是否需要将时间与模拟同步

yaml_filename：地图的yaml文件的路径

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

2、对于启动lifecycle_manager节点

此节点关于导航中涉及的节点的生命周期

需要的参数：

1、use_sim_time

2、autostart：布尔值，指示生命周期管理器在启动时是否必须启动

3、node_names：是一个包含生命周期管理器必须处理的节点名称的列表。目前为止只有map_server。

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

练习2.3

创建map_server启动文件的步骤

在ros2_ws中创建map_server包

在该包中创建一个启动目录，叫做nav2_map_server.launch.py

在启动文件中包含上面提到的两个节点

在该包中创建一个config路径放地图

启动map_server，载入config中的地图，并且可视化这个地图。

笔记

记得像2.1练习一样编辑setup.py

记住在RVIZ中检查/map主题

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']}])            
        ])

Nav2 Lifecycle Manager

导航节点是被管理的节点，它们可以被容易的重启、暂停或者运行。被管控的节点通过以下任一状态来控制

unconfigured未配置的、inactive不活跃的、active活跃的、finalized已敲定的

 被管理的节点通常应该处于active状态，即当节点运行它的主代码并且执行它的计时功能时。

被管理的节点从unconfigured状态开始，为了让状态从unconfigured到active，节点需要一个外部代理将其移动到新状态。

一些Nav2中的节点，比如map_server,amcl,planner_server等都支持证明周期，因此它们是被管理节点。

这些节点提供了生命周期函数所需的覆盖。on_configure() , on_activate() , on_deactivate() , on_cleanup() , on_shutdown() , and on_error() .

Nav2中的生命周期管理

再Nav2中激活所有导航节点的节点成为Nav2_lifecycle_manager

它可以改变受管理节点的状态，以实现导航堆栈的受控启动、关闭、重置、暂停或者恢复

Nav2使用lifecyclenode的包装器，nav2_util lifecycleNode.这个包装器隐藏了典型应用程序lifecyclenode的许多复杂性。它还包括一个用于生命周期管理器的绑定链接，以确保在节点向上转换后，它也保持活动状态。当节点崩溃时，它会让生命周期管理器知道并向下转移系统，以防止出现严重故障。

可以从程序中调用该服务，用受控方式重新启动、停用导航。

nav2_lifecycle_manager需要一系列被管理节点列表，如下

Node(
    package='nav2_lifecycle_manager',
    executable='lifecycle_manager',
    name='lifecycle_manager',
    output='screen',
    parameters=[{'autostart': True},
                {'node_names': ['map_server',
                                'amcl',
                                'controller_server',
                                'planner_server',
                                'recoveries_server',
                                'bt_navigator']}])

它将使用node_names列表以及该列表中的顺序来标识要管理的节点。初始化的顺序是从前往后，暂停、停止的顺序是从后到前。

确保autostart为true。

验证导航系统

关闭一切程序

在第一个终端中

cd ~/ros2_ws/

colcon build

source install/setup.bash

ros2 launch map_server nav2_map_server.launch.py

接着检查nav2_lifecycle_manager正在运行的服务。为此需要打开另一个终端并运行

ros2 service list | grep lifecycle

需要得到以下结果

/lifecycle_manager_mapper/describe_parameters
/lifecycle_manager_mapper/get_parameter_types
/lifecycle_manager_mapper/get_parameters
/lifecycle_manager_mapper/is_active
/lifecycle_manager_mapper/list_parameters
/lifecycle_manager_mapper/manage_nodes
/lifecycle_manager_mapper/set_parameters
/lifecycle_manager_mapper/set_parameters_atomically

如上所示，提供了manage_nodes服务

接下来看需要调用哪种消息类型。

ros2 service type /lifecycle_manager_mapper/manage_nodes

输出

nav2_msgs/srv/ManageLifecycleNodes

接着输入

ros2 interface show nav2_msgs/srv/ManageLifecycleNodes

 输出是

uint8 STARTUP = 0
uint8 PAUSE = 1
uint8 RESUME = 2
uint8 RESET = 3
uint8 SHUTDOWN = 4

uint8 command
---
bool success

调用服务时，必须提供一个0-4之间的数字，指示要将导航系统置于哪个状态。服务将返回一个布尔值指示是否成果。比如你可以使用以下消息调用该服务来达到暂停导航系统的服务。

ros2 service call /lifecycle_manager_mapper/manage_nodes nav2_msgs/srv/ManageLifecycleNodes command:\ 1\

会得到如下应答

requester: making request: nav2_msgs.srv.ManageLifecycleNodes_Request(command=1)

response:
nav2_msgs.srv.ManageLifecycleNodes_Response(success=True)

这时回看终端1会得到如下消息

[lifecycle_manager-2] [INFO] [1655285986.303567128] [lifecycle_manager_mapper]: Terminating bond timer...
[lifecycle_manager-2] [INFO] [1655285986.303659326] [lifecycle_manager_mapper]: Pausing managed nodes...
[lifecycle_manager-2] [INFO] [1655285986.303698330] [lifecycle_manager_mapper]: Deactivating map_server
[map_server-1] [INFO] [1655285986.307166044] [map_server]: Deactivating
[map_server-1] [INFO] [1655285986.307227673] [map_server]: Destroying bond (map_server) to lifecycle manager.
[lifecycle_manager-2] [INFO] [1655285986.412322210] [lifecycle_manager_mapper]: Managed nodes have been paused

如果要恢复导航，在终端2中

ros2 service call /lifecycle_manager_mapper/manage_nodes nav2_msgs/srv/ManageLifecycleNodes command:\ 2\