Welcome to MicroROS Robot repository

Navigation2 navigation avoid

1、Introduction to Navigation2

Navigation2 overall architecture diagram

Navigation2 has the following tools:

• Tools for loading, serving and storing maps (Map Server)
• Tools for locating robots on a map (AMCL)
• Path planning tool (Nav2 Planner) to avoid obstacles and move from point A to point B
• Tool to control the robot while following the path (Nav2 Controller)
• Tool to convert sensor data into cost map representation in the robotic world (Nav2 Costmap 2D)
• Tools for building complex robot behaviors using behavior trees (Nav2 Behavior Tree and BT Navigator)
• Tools for calculating recovery behavior in the event of failure (Nav2 Recoveries)
• Tool for following sequential waypoints (Nav2 Waypoint Follower)
• Tools and watchdogs for managing server lifecycle (Nav2 Lifecycle Manager)
• Plug-in to enable user-defined algorithms and behaviors (Nav2 Core)

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

Use Behavior Trees (BT, Behavior Trees) to call the modular server 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 the Behavior Tree (BT) through the action server.

Information reference website:

Navigation2 Documentation：https://navigation.ros.org/index.html

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

Papers corresponding to Navigation2：https://arxiv.org/pdf/2003.00368.pdf

Plug-ins provided by Navigation2：https://navigation.ros.org/plugins/index.html#plugins

2. Program function description

The car connects to the agent, runs the program, and the map will be loaded in rviz. In the rviz interface, use the [2D Pose Estimate] tool to give the initial pose of the car, 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 an obstacle during the process, it will avoid the obstacle by itself and stop when it reaches the destination.

3. Start and connect to the agent

Taking 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 8090 -v4

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

4. Start the program

If you are using the Raspberry Pi desktop version or the Jetson Nano desktop version, you need to enter Docker beforehand by typing the following in the terminal:

xxxxxxxxxx
sh ros2_humble.sh

The following interface indicates that you have successfully entered Docker:

Then, in Docker, enter the following commands: (See the "Docker Environment" section for how to access the same Docker terminal)

xxxxxxxxxx
ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py      # Low-level data processing program
ros2 launch yahboomcar_nav display_launch.py            # RViz visualization

First, start the low-level data processing program for the robot car. Taking the provided virtual machine as an example, enter the following in the terminal:

xxxxxxxxxx
ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

Then, start rviz, visual navigation, and terminal input.

xxxxxxxxxx
ros2 launch yahboomcar_nav display_launch.py

At this time, the map loading is not displayed because the navigation program has not been started yet, so there is no map loading. Next run the navigation node and enter in the terminal.

For the virtual machine example, enter the following in the terminal:

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

Map parameter loading: maps:=/home/yahboom/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml (replaceable with the target map)

For the Jetson Nano version, enter the following in the terminal:

xxxxxxxxxx
ros2 launch yahboomcar_nav navigation_dwb_launch.py maps:=/root/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml

Map parameter loading: maps:=/root/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml (replaceable with the target map)

For the Raspberry Pi version, enter the following in the terminal:

xxxxxxxxxx
ros2 launch  yahboomcar_nav navigation_dwb_launch.py maps:=/root/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml

Map parameter loading: maps:=/root/yahboomcar_ws/src/yahboomcar_nav/maps/testaa.yaml (replaceable with the target map)

At this point you can see that the map is loaded, and then we click [2D Pose Estimate] to set the initial pose for the car. According to the position of the car in the actual environment, click and drag with the mouse in rviz, and the car model moves according to the position we set Location. As shown in the figure below, if the area scanned by the radar roughly coincides with the actual obstacle, it means the pose is accurate.

For single-point navigation, click the [2D Goal Pose] tool, and then select a target point in rviz. The car will plan a path based on the surrounding situation and move along the path to the target point.

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

Use [Nav2 Goal] in the rivz toolbar to specify any target point, and then click [Start Waypoint Following] to start planning path navigation. The car will automatically go to the next point according to the order of the selected points, and no operation is required after reaching the target point. After reaching the last point, the car stops and waits for the next instruction.

5. View the node communication diagram

Terminal input,

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

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

6. View TF tree

Terminal input,

xxxxxxxxxx
ros2 run tf2_tools view_frames

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

7. Code analysis

Here we only describe the navigation_dwb_launch.py of navigation.

Taking a virtual machine as an example, the file path is:

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

Jetson Nano code path:

xxxxxxxxxx
/root/yahboomcar_ws/src/yahboomcar_nav/launch

Raspberry Pi code path:

xxxxxxxxxx
/root/yahboomcar_ws/src/yahboomcar_nav/launch

navigation_dwb_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')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
​
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    map_yaml_path = LaunchConfiguration(
        'maps', default=os.path.join(package_path, '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('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(
                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
        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']
        ),
        Node(
            package='yahboomcar_nav',
            executable='stop_car'
        ) 
    ])

The following nodes are started here.

• base_link_to_base_laser：Publish static TF transformation;
• stop_car：For the parking node, after ctrl c exits the program, the parking speed will be announced to the car;
• bringup_launch.py：Launch navigation launch file, the file is located at, /opt/ros/humble/share/nav2_bringup/launch

In addition, a navigation parameter configuration file dwb_nav_params.yaml and a map file yahboom_map.yaml are also loaded. The navigation parameter table is located at,

Taking the virtual machine as an example, the navigation parameter files are located at:

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

The map file is located at:

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

Jetson Nano code path:

xxxxxxxxxx
/root/yahboomcar_ws/src/yahboomcar_nav/params

The map file is located at:

xxxxxxxxxx
/root/yahboomcar_ws/src/yahboomcar_nav/maps

Raspberry Pi code path:

xxxxxxxxxx
/root/yahboomcar_ws/src/yahboomcar_nav/params

The map file is located at:

xxxxxxxxxx
/root/yahboomcar_ws/src/yahboomcar_nav/maps

dwb_nav_params.yaml，

xxxxxxxxxx
amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan
​
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
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_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
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: -0.20
      min_vel_y: 0.0
      max_vel_x: 0.30
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: -0.20
      max_speed_xy: 0.30
      min_speed_theta: -0.5
      # 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: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      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
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 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.22
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      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.22
      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.55
      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.1
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 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.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.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 launched in the navigation launch file.