Robot Programming Fun Practice 17-Chaotic Memories-

In the previous section, SLAM was involved in building a map. Is this perfect? ​​The maps in the simulation are so complete.

Actually, it's not. Look at the following dynamic picture, the cartographer effect:

There are good moments, and there are lost moments.

If you need an in-depth understanding of the cartographer algorithm, a comprehensive reading of the paper, source code and documentation is recommended.

  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

It is recommended to download the latest pdf and read it carefully!

Cartographer ROS integration

Cartographer is a system that provides 2D and 3D real-time simultaneous localization and mapping (SLAM) across multiple platforms and sensor configurations. This project provides Cartographer's ROS integration.

A good map can make navigation more "silky"!

Of course ROS2 provides more powerful SLAM tools, such as: slam_toolbox

  • github.com/SteveMacenski/slam_toolbox

Introduction

Slam Toolbox is a set of tools and functions for 2D SLAM, built by Steve Macenski during Simbe Robotics and maintained at Samsung Research Institute.

The project contains the ability to execute most of the content of any other available SLAM library (free, paid, etc.). This includes:

  1. Ordinary fool-like 2D SLAM mobile robot technicians expect (start, create maps, save pgm files) to have some good built-in utilities, such as saving maps
  2. Continue to optimize, rebuild the image or continue to create the saved (serialized) pose image at any time
  3. Full life cycle mapping: load the saved pose image and continue to create the image in the space, and at the same time delete irrelevant information from the newly added scan
  4. An optimized positioning mode based on the posture map. You can choose to run the positioning mode without a priori map for the "lidar odometer" mode with a partial closed loop
  5. Synchronous and asynchronous mapping modes
  6. Motion graph merging (use elastic graph operation merging technology in work)
  7. Plug-in based optimization solver with new optimized Google Ceres-based plug-in
  8. RVIZ plug-in for interacting with tools
  9. Graphical manipulation tool in RVIZ for manipulating nodes and connections during mapping
  10. Map serialization and lossless data storage
  11. ...More, but these are the highlights

For running on an on-site production robot, it is recommended to use snap: slam-toolbox, which has been optimized to increase its speed by approximately 10 times. For other developer-level tools (rviz plugins, etc.) that do not need to be on the robot, deb/source installation is required.

The software package has been benchmarked in 5x+ real-time up to approximately 30,000 square feet and 3x real-time surveying buildings up to approximately 60,000 square feet. The maximum area used is 200,000 square feet. Build in synchronous mode (that is, handle all scans, regardless of latency), and more space in asynchronous mode.

Support simultaneous navigation and map creation!

Map building and navigation are carried out simultaneously, and the efficiency is greatly improved!

import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescriptionfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescriptionfrom launch.conditions import IfConditionfrom launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch.substitutions import LaunchConfiguration, PythonExpressionfrom 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

Full life cycle mapping

LifeLong mapping is a concept that can fully or partially map a space, and as time goes by, as time continues to interact with the space, the map will be improved and updated. Pay attention to applications that are allowed to operate in the cloud, and map creation with many robots in a shared space (cloud distributed map creation). Although Slam Toolbox can also be used for idiot mapping of space and save the map as a .pgm file, because traditionally stores the map, it also allows to save the pose map and metadata losslessly to reload later using the same or different The robot continues to build the map space.

The whole life cycle mapping includes several key steps

  • Serialization and deserialization to store and reload map information
  • KD-Tree searches for matches to locate the robot in its position when reinitializing
  • SLAM and 2D scanning matching abstraction based on posture graph optimization

This will allow users to create and update existing maps, and then serialize the data for use in other map sessions, which is severely lacking in most SLAM implementations and almost all planar SLAM implementations. Other good libraries for doing this include RTab-Map and Cartorapher, although they have their own quirks that make them (in my opinion) unusable for production robot applications. The mechanism provided by the library can not only save data, but also save posture maps and related metadata. This has been used through merging technology (using 2 or more serialized objects and creating a globally consistent object) and continuous mapping technology (updating and performing the same serialized mapping object over time) Improved) to create a map. Compared with RTab-Map or Cartoprapher, its main advantage lies in the maturity of the underlying (but heavily modified) open_karto library on which the project is based. Karto's scan matcher is a well-known and very good 2D laser scan matcher, and modified versions of Karto can be found in companies all over the world.

Slam Toolbox supports all major modes:

  • Start from a predefined dock (assuming it is near the start area)
  • Start from any specific node-select the node ID you want to start nearby
  • Start from any specific area-indicate the current pose to start in the map frame, such as AMCL

In the RVIZ interface (see the section below), you will be able to reposition in the map or use the ROS service to continue the mapping graphically or programmatically.

At the time of writing: There is a highly experimental implementation called "true full life cycle" mapping. It does support the method of deleting nodes and adding nodes over time, which leads to true life-long mapping capabilities. Because calculations are bounded by deleting irrelevant or outdated information. If you want to continuously optimize the map, it is recommended to run the incomplete LifeLong mapping mode in the cloud to increase the computational burden. However, a real and urgently needed application is to use multi-session mapping to update a part of the map or map half an area at a time, to create a complete (and then static) map for AMCL or Slam Toolbox localization mode, which will be used in spades. In the treatment. The near-term plan is to create a pattern in the LifeLong map to attenuate old nodes to limit calculations, and refine experimental nodes to make them run on the edge. You should use continuous mapping (lifetime) to build a complete map, and then switch to the posture map deformation positioning mode until the node attenuation is achieved, and you should not see any substantial performance impact.

For more information, please refer to the official document!