机器人编程趣味实践17-混乱的记忆-

Posted 2021-06-27 zhangrelay

上一节，涉及了SLAM构建地图，是不是这样就完美了，仿真中的地图都那么完整呢。

其实，并不是，看如下动态图，cartographer效果：

有美好的时刻，也有迷茫的时光。

如果需要深入了解cartographer算法，推荐论文、源码和文档的综合阅读。

1. github.com/ros2/cartographer
2. github.com/ros2/cartographer_ros
3. google-cartographer.readthedocs.io/en/latest/
4. google-cartographer-ros.readthedocs.io/en/latest/index.html

推荐下载最新pdf，仔细阅读！

Cartographer ROS 集成

Cartographer 是一个系统，可提供跨多个平台和传感器配置的 2D 和 3D 实时同步定位和地图构建 (SLAM)。 该项目提供 Cartographer 的 ROS 集成。

有个好地图，才能导航更“丝滑”！

当然ROS2提供了更多强大的SLAM工具，比如：slam_toolbox

• github.com/SteveMacenski/slam_toolbox

介绍

Slam Toolbox 是一组用于 2D SLAM 的工具和功能，由 Steve Macenski 在 Simbe Robotics 期间构建，在三星研究院维护。

项目包含执行任何其他可用 SLAM 库（免费和付费等）的大部分内容的能力。这包括：

1.     普通的傻瓜式 2D SLAM 移动机器人技术人员期望（启动、建图、保存 pgm 文件）具有一些不错的内置实用程序，例如保存地图
2.     随时继续优化、重新建图或继续建图已保存（序列化）的姿势图
3.     全生命周期建图：加载保存的姿势图在空间中继续建图，同时还从新添加的扫描中删除无关信息
4.     建立在姿势图上的基于优化的定位模式。可选择在没有先验地图的情况下运行定位模式，用于具有局部闭环的“激光雷达里程计”模式
5.     同步和异步建图模式
6.     运动图合并（在工作中使用弹性图操作合并技术）
7.     基于插件的优化求解器，带有新的优化的基于 Google Ceres 的插件
8.     用于与工具交互的 RVIZ 插件
9.     RVIZ 中的图形操作工具，用于在映射期间操作节点和连接
10.     地图序列化和无损数据存储
11.     ...更多，但这些是亮点

对于在现场生产机器人上运行，建议使用 snap: slam-toolbox，它进行了优化，使其速度提高了大约 10 倍。对于不需要在机器人上的其他开发人员级工具（rviz 插件等），需要 deb/source 安装。

该软件包已在高达约 30,000 平方英尺的 5x+ 实时和高达约 60,000 平方英尺的 3x 实时测绘建筑中进行了基准测试。使用的最大面积是 200,000 平方英尺。在同步模式下构建（即处理所有扫描，无论延迟如何），以及在异步模式下更大的空间。

支持同时导航和建图！

构建地图和导航同步进行，效率提升非常多！

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node


def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('nav2_bringup')
    launch_dir = os.path.join(bringup_dir, 'launch')

    # Create the launch configuration variables
    slam = LaunchConfiguration('slam')
    namespace = LaunchConfiguration('namespace')
    use_namespace = LaunchConfiguration('use_namespace')
    map_yaml_file = LaunchConfiguration('map')
    use_sim_time = LaunchConfiguration('use_sim_time')
    params_file = LaunchConfiguration('params_file')
    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
    autostart = LaunchConfiguration('autostart')

    # Launch configuration variables specific to simulation
    rviz_config_file = LaunchConfiguration('rviz_config_file')
    use_simulator = LaunchConfiguration('use_simulator')
    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
    use_rviz = LaunchConfiguration('use_rviz')
    headless = LaunchConfiguration('headless')
    world = LaunchConfiguration('world')

    # Map fully qualified names to relative ones so the node's namespace can be prepended.
    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
    # https://github.com/ros/geometry2/issues/32
    # https://github.com/ros/robot_state_publisher/pull/30
    # TODO(orduno) Substitute with `PushNodeRemapping`
    #              https://github.com/ros2/launch_ros/issues/56
    remappings = [('/tf', 'tf'),
                  ('/tf_static', 'tf_static')]

    # Declare the launch arguments
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace')

    declare_use_namespace_cmd = DeclareLaunchArgument(
        'use_namespace',
        default_value='false',
        description='Whether to apply a namespace to the navigation stack')

    declare_slam_cmd = DeclareLaunchArgument(
        'slam',
        default_value='False',
        description='Whether run a SLAM')

    declare_map_yaml_cmd = DeclareLaunchArgument(
        'map',
        default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
        description='Full path to map file to load')

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true')

    declare_params_file_cmd = DeclareLaunchArgument(
        'params_file',
        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
        description='Full path to the ROS2 parameters file to use for all launched nodes')

    declare_bt_xml_cmd = DeclareLaunchArgument(
        'default_bt_xml_filename',
        default_value=os.path.join(
            get_package_share_directory('nav2_bt_navigator'),
            'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
        description='Full path to the behavior tree xml file to use')

    declare_autostart_cmd = DeclareLaunchArgument(
        'autostart', default_value='true',
        description='Automatically startup the nav2 stack')

    declare_rviz_config_file_cmd = DeclareLaunchArgument(
        'rviz_config_file',
        default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
        description='Full path to the RVIZ config file to use')

    declare_use_simulator_cmd = DeclareLaunchArgument(
        'use_simulator',
        default_value='True',
        description='Whether to start the simulator')

    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
        'use_robot_state_pub',
        default_value='True',
        description='Whether to start the robot state publisher')

    declare_use_rviz_cmd = DeclareLaunchArgument(
        'use_rviz',
        default_value='True',
        description='Whether to start RVIZ')

    declare_simulator_cmd = DeclareLaunchArgument(
        'headless',
        default_value='False',
        description='Whether to execute gzclient)')

    declare_world_cmd = DeclareLaunchArgument(
        'world',
        # TODO(orduno) Switch back once ROS argument passing has been fixed upstream
        #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
        # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
        #                            'worlds/turtlebot3_worlds/waffle.model'),
        default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
        description='Full path to world model file to load')

    # Specify the actions
    start_gazebo_server_cmd = ExecuteProcess(
        condition=IfCondition(use_simulator),
        cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
        cwd=[launch_dir], output='screen')

    start_gazebo_client_cmd = ExecuteProcess(
        condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])),
        cmd=['gzclient'],
        cwd=[launch_dir], output='screen')

    urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')

    start_robot_state_publisher_cmd = Node(
        condition=IfCondition(use_robot_state_pub),
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        namespace=namespace,
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        remappings=remappings,
        arguments=[urdf])

    rviz_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
        condition=IfCondition(use_rviz),
        launch_arguments={'namespace': '',
                          'use_namespace': 'False',
                          'rviz_config': rviz_config_file}.items())

    bringup_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(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,
                          'default_bt_xml_filename': default_bt_xml_filename,
                          'autostart': autostart}.items())

    # Create the launch description and populate
    ld = LaunchDescription()

    # Declare the launch options
    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_namespace_cmd)
    ld.add_action(declare_slam_cmd)
    ld.add_action(declare_map_yaml_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_params_file_cmd)
    ld.add_action(declare_bt_xml_cmd)
    ld.add_action(declare_autostart_cmd)

    ld.add_action(declare_rviz_config_file_cmd)
    ld.add_action(declare_use_simulator_cmd)
    ld.add_action(declare_use_robot_state_pub_cmd)
    ld.add_action(declare_use_rviz_cmd)
    ld.add_action(declare_simulator_cmd)
    ld.add_action(declare_world_cmd)

    # Add any conditioned actions
    ld.add_action(start_gazebo_server_cmd)
    ld.add_action(start_gazebo_client_cmd)

    # Add the actions to launch all of the navigation nodes
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(rviz_cmd)
    ld.add_action(bringup_cmd)

    return ld

全生命周期建图

LifeLong 建图是能够完全或部分建图空间的概念，并且随着时间的推移，随着继续与空间交互，完善和更新该地图。注意允许在云中操作的应用程序，以及在共享空间中与许多机器人进行建图（云分布式建图）。虽然 Slam Toolbox 也可以用于空间的傻瓜式映射并将该地图保存为 .pgm 文件，因为传统上存储地图，但它还允许无损地保存姿势图和元数据以重新加载稍后使用相同或不同的机器人并继续建图空间。

全生命周期建图包括几个关键步骤

•     序列化和反序列化以存储和重新加载地图信息
•     KD-Tree 搜索匹配以在重新初始化时将机器人定位在其位置
•     基于姿势图优化的 SLAM 与 2D 扫描匹配抽象

这将允许用户创建和更新现有地图，然后序列化数据以用于其他地图会话，这是大多数 SLAM 实现和几乎所有平面 SLAM 实现所严重缺乏的。其他这样做的好库包括 RTab-Map 和 Cartorapher，尽管它们本身有自己的怪癖，使它们（在我看来）无法用于生产机器人应用程序。该库提供的机制不仅可以保存数据，还可以保存姿势图和相关的元数据。这已被用于通过合并技术（采用 2 个或更多序列化对象并创建 1 个全局一致的对象）以及连续映射技术（随着时间的推移更新 1 个相同的序列化映射对象并对其进行改进）来创建地图。与 RTab-Map 或 Cartoprapher 相比，它的主要优势在于项目所基于的底层（但经过大量修改）open_karto 库的成熟度。 Karto 的扫描匹配器是众所周知的非常好的 2D 激光扫描匹配器，并且在世界各地的公司中都可以找到 Karto 的修改版本。

Slam Toolbox 支持所有主要模式：

•     从预定义的码头开始（假设在开始区域附近）
•     从任何特定节点开始 - 选择要在附近开始的节点 ID
•     从任何特定区域开始 - 在地图框中指示要开始的当前姿势，例如 AMCL

在 RVIZ 界面（请参阅下面的部分）中，将能够在地图中重新定位或使用 ROS 服务以图形方式或编程方式继续映射。

在撰写本文时：有一个称之为“真正全生命周期”建图的高度实验性实现，它确实支持随着时间的推移删除节点以及添加节点的方法，这导致了真正的终身建图能力，因为计算是有界的通过删除无关或过时的信息。如果想不断优化地图，建议在云中运行非完整 LifeLong 建图模式以增加计算负担。然而，一个真正且迫切需要的应用是使用多会话映射来更新地图的一部分或一次映射半个区域，为 AMCL 或 Slam Toolbox 本地化模式创建完整的（然后是静态的）地图，这将在黑桃中处理。近期的计划是在 LifeLong 映射中创建一种模式，以衰减旧节点以限制计算，并通过细化实验节点使其在边缘上运行。应该使用持续映射（终身）来构建完整的地图，然后切换到姿势图变形定位模式，直到实现节点衰减，应该不会看到任何实质性的性能影响。

更多内容参考官方文档！