Navigation2 fast relocation navigation

1. Navigation2 introduction

Navigation2 overall architecture diagram

Navigation2 has the following tools:

• Tools for loading, providing and storing maps (Map Server)

• Tools for locating robots on maps (AMCL)

• Path planning tools for moving from point A to point B while avoiding obstacles (Nav2 Planner)

• Tools for controlling robots while following paths (Nav2 Controller)

• Tools for converting sensor data into cost map expressions in the robot world (Nav2 Costmap 2D)

• Tools for building complex robot behaviors using behavior trees (Nav2 Behavior Trees and BT Navigator)

• Tools for calculating recovery behaviors when failures occur (Nav2 Recoveries)

• Tools for following sequential waypoints (Nav2 Waypoint Follower)

• Tools and watchdogs for managing server lifecycles (Nav2 Lifecycle Manager)

• Plugins to enable user-defined algorithms and behaviors (Nav2 Core)

Navigation 2 (Nav 2) is the navigation framework included with ROS 2. Its purpose is to enable a mobile robot to move from point A to point B in a safe way. Therefore, Nav 2 can perform behaviors such as dynamic path planning, calculating motor speeds, avoiding obstacles, and recovering structures.

Nav 2 uses behavior trees (BT) to call modular servers to complete an action. Actions can be path calculations, control efforts, recovery, or other navigation-related actions. These actions are independent nodes that communicate with behavior trees (BT) through action servers.

Reference URL:

Navigation2 Document: https://navigation.ros.org/index.html

Navigation2 github: https://github.com/ros-planning/navigation2

Navigation2 Corresponding Paper: https://arxiv.org/pdf/2003.00368.pdf

Plugins provided by Navigation2: https://navigation.ros.org/plugins/index.html#plugins

2. Program Function Description

The car connects to the proxy, runs the program, and the map will be loaded in rviz. In the rviz interface, use the [2D Pose Estimate] tool to give the car an initial pose, and then use the [2D Goal Pose] tool to give the car a target point. The car will plan a path based on its own environment and move to the destination according to the planned path. If it encounters obstacles during the process, it will avoid obstacles by itself and stop after reaching the destination.

xxxxxxxxxx
Note: Before running the program, the car needs to be restarted in a stable standing position to ensure that all sensors are reset

3. Start and connect the agent

Take the supporting virtual machine as an example, enter the following command to start the agent,

xxxxxxxxxx
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 8899 -v4

Then, turn on the car switch and wait for the car to connect to the proxy. The connection is successful as shown in the figure below.

4. Start the program

4.1 Run the command

First, start the car to process the underlying data program. Take the matching virtual machine as an example. Enter the terminal,

xxxxxxxxxx
ros2 launch yahboomcar_nav navigation_bringup_launch.py

Then, start fast relocation and wait for rviz to be automatically opened.

xxxxxxxxxx
ros2 launch yahboomcar_nav localization_imu_odom.launch.py ••use_rviz:=true load_state_filename:=/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.pbstream

Parameter Description

xxxxxxxxxx
#Select whether to open rviz, true is open, false is not open
use_rviz:=true
#Replaceable target map file
#.pbstream map file refers to cartograph mapping algorithm to save the map
load_state_filename:=/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.pbstream

At this time, you can see that the map is loaded, but the car is not accurately positioned. Then we click [2D Pose Estimate] to set the initial pose for the car. According to the approximate position of the car in the actual environment, click and drag with the mouse in rviz, and the car can be quickly positioned on the map. As shown in the figure below, the area scanned by the radar roughly coincides with the actual obstacle.

Note: If it still cannot be positioned, it may be that the car is in an obstacle area. Please place it in a place with no obstacles around it and start again. You can also keep the terminal open, restart the car directly, wait for the data to be automatically restored and then reposition. It may also be that there are too few map feature points when building the map, and the map must be built more carefully.

Next, run the navigation node,

Take the virtual machine as an example, input in the terminal,

xxxxxxxxxx
ros2 launch yahboomcar_nav navigation_cartodwb_launch.py ••maps:=/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml

Parameter description

xxxxxxxxxx
#Load map parameters: (target map can be replaced)
maps:=/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml
#.pgm file must also be in the same path as .yaml

Note: Here, testaa.yaml and testaa.pbstream must be mapped at the same time, that is, the same map, refer to cartograph mapping algorithm to save the map

Single-point navigation, click the [2D Goal Pose] tool, then select a target point in rviz, the car plans a path based on the surrounding situation and moves to the target point along the path.

For multi-point navigation, you need to add the nav2 plug-in.

After adding, rviz displays as follows.

Then click [Waypoint/Nav Through Poses Mode] and use the [Nav2 Goal】Give any target point,

Then click [Start Waypoint Following] to start planning path navigation. The car will automatically go to the next point after reaching the target point according to the order of the selected points, without any operation. After reaching the last point, the car stops and waits for the next instruction.

5. View the node communication graph

Terminal input,

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

If it is not displayed at the beginning, select [Nodes/Topics(all)] and click the refresh button in the upper left corner.

6. View TF tree

Terminal input,

xxxxxxxxxx
ros2 run tf2_tools view_frames

After running, two files will be generated in the terminal directory, namely .gv and .pdf files, and the pdf file is the TF tree.

7. Code analysis

Take the virtual machine as an example, the file path is,

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/launch

Here is a description of how to quickly relocate localization_imu_odom.launch.py.

xxxxxxxxxx
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import Shutdown

def generate_launch_description():

    load_state_filename_arg = DeclareLaunchArgument(
        'load_state_filename',
        default_value='/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.pbstream'
        )

    use_rviz_arg = DeclareLaunchArgument(
        'use_rviz',
        default_value='true',
    )

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': False}],
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'ros1_backpack_2d_localization_imu_odom.lua',
            '-load_state_filename', LaunchConfiguration('load_state_filename')],
        remappings = [
            ('imu', '/imu/data')],
        output = 'screen'
        )

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': False},
            {'resolution': 0.05}],
        )
    
    base_link_to_laser_tf_node = Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     name='base_link_to_base_laser',
     arguments=['0.0', '0.0', '0.138', '0', '0', '0', 'base_link', 'laser_frame']
    )
    
    rviz_node = Node(
        package = 'rviz2',
        executable = 'rviz2',
        on_exit = Shutdown(),
        arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'],
        parameters = [{'use_sim_time': False}],
        condition=IfCondition(LaunchConfiguration('use_rviz'))
    )

    return LaunchDescription([
        # Launch arguments
        load_state_filename_arg,
        use_rviz_arg,
        # Nodes
        base_link_to_laser_tf_node,
        cartographer_node,
        cartographer_occupancy_grid_node,
        rviz_node,
    ])

The following nodes are started here:

• base_link_to_laser_tf_node: publishes the static TF transformation from base_link to laser_frame;

• load_state_filename_arg: declares a startup parameter load_state_filename, the default value is /home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.pbstream, which is used to specify the map file path to be loaded;

• cartographer_node: starts the cartographer_node node in the cartographer_ros package, which is responsible for processing SLAM (simultaneous localization and mapping);

• cartographer_occupancy_grid_node: starts the cartographer_occupancy_grid_node node in the cartographer_ros package, which is used to generate an occupancy grid map, and sets the resolution to 0.05 meters.

Navigation node navigation_cartodwb_launch.py,

xxxxxxxxxx
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    package_path = get_package_share_directory('yahboomcar_nav')

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    namespece = LaunchConfiguration('namespece', default='')
    map_yaml_path = LaunchConfiguration(
        'maps', default=os.path.join('/home/yahboom/yahboomcar_ws/src/yahboomcar_nav', 'maps', 'yahboom_map.yaml')) 
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        package_path, 'params', 'dwb_nav_params.yaml'))

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                              description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument('namespece', default_value=namespece,
                              description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument('maps', default_value=map_yaml_path,
                              description='Full path to map file to load'),
        DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                              description='Full path to param file to load'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [package_path, '/launch', '/cartographer_bringup_launch.py']),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'namespece': namespece,
                'params_file': nav2_param_path}.items(),
        ),
        Node(
            package='yahboomcar_nav',
            executable='stop_car'
        ) ,
        Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     name='base_link_to_base_laser',
     arguments=['-0.0046412', '0' , '0.094079','0','0','0','base_link','laser_frame']
    )
    ])

The following nodes are started here:

• base_link_to_laser_tf: publish static TF transformation;

• map_yaml_path: load map file, the default value is /home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.yaml, which is used to specify the map file path to be loaded;

• stop_car: parking node, after ctrl c exits the program, the parking speed will be published to the car;

• cartographer_bringup_launch.py: start the launch file of cartographer pure positioning, the file is located in, /home/yahboom/yahboomcar_ws/src/yahboomcar_nav/launch/cartographer_bringup_launch.py

In addition, a navigation parameter configuration file dwb_nav_params.yaml is loaded.

The map file is located in,

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps

Take the virtual machine as an example, the navigation parameter table is located at,

xxxxxxxxxx
/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/params

dwb_nav_params.yaml，

xxxxxxxxxx
bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    default_bt_xml_filename: "navigate_to_pose_w_replanning_and_recovery.xml"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node

bt_navigator_navigate_through_poses_rclcpp_node:
  ros__parameters:
    use_sim_time: False

bt_navigator_navigate_to_pose_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001 #0.01
    max_theta_velocity_threshold: 5.0 #0.5
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker" 
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    precise_goal_checker:
     plugin: "nav2_controller::SimpleGoalChecker"
     xy_goal_tolerance: 0.25 #0.25 
     yaw_goal_tolerance: 0.15 #0.15 
     stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25 #0.25 
      yaw_goal_tolerance: 0.15 #0.15
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: -0.50
      min_vel_y: 0.0
      max_vel_x: 0.55
      max_vel_y: 0.0
      max_vel_theta: 10.0
      min_speed_xy: -0.50
      max_speed_xy: 0.55
      min_speed_theta: -10.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 5.2 #4.5
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -5.2 #-4.5
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.25
      xy_goal_tolerance: 0.2
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0 #24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 48.0 #32.0
      RotateToGoal.slowing_factor: 0.5 #5.0
      RotateToGoal.lookahead_time: -1.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.15
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.18
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      robot_radius: 0.15
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.18
      always_send_full_costmap: True

map_server:
  ros__parameters:
    use_sim_time: False
    # Overridden in launch by the "map" launch configuration or provided default value.
    # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
    yaml_filename: ""

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true
smoother_server:
  ros__parameters:
    use_sim_time: False
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: False

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.15
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 5.0 #1.0
    min_rotational_vel: 4.0 #0.4
    rotational_acc_lim: 3.2    #3.2 

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

waypoint_follower:
  ros__parameters:
    use_sim_time: False
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    use_sim_time: False
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.35, 0.0, 3.0]
    min_velocity: [-0.35, 0.0, -3.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

This parameter table configures the parameters required for each node started in the navigation launch file.