Welcome to ROSMASTER M1 repository

Navigation2 single-point navigation avoid

1. Program Functionality

After running the program, a map will load in rviz. In the rviz interface, use the [2D Pose Estimate] tool to set the car's initial pose, and then use the [2D Goal Pose] tool to set a target point. The car will then calculate a path based on its surroundings and move to its destination. If it encounters obstacles, it will automatically avoid them and stop at its destination.

2. Navigation2 Introduction

2.1 Introduction

Navigation2 Overall Architecture

Navigation2 provides the following tools:

• Tools for loading, serving, and storing maps (Map Server)
• Tools for localizing the robot on a map (AMCL)
• Tools for planning paths from point A to point B while avoiding obstacles (Nav2 Planner)
• Tools for controlling the robot while following a path (Nav2 Controller)
• Tools for converting sensor data into a costmap representation of the robot's world (Nav2 Costmap 2D)
• Tools for building complex robot behaviors using behavior trees (Nav2 Behavior Trees and BT Navigator)
• Tools for calculating recovery behaviors in the event of a failure (Nav2 Recoveries)
• Tools for following sequential waypoints (Nav2 Waypoint Follower)
• Tools and watchdogs for managing server lifecycles (Nav2 Lifecycle Manager)
• Plugins for enabling user-defined algorithms and behaviors (Nav2 Core)

Navigation 2 (Nav2) is the native navigation framework in ROS 2. Its purpose is to safely move mobile robots from point A to point B. Nav2 implements behaviors such as dynamic path planning, motor speed calculation, obstacle avoidance, and structure recovery.

Nav2 uses behavior trees (BTs) to call modular servers to perform an action. Actions can include path calculation, control efforts, recovery, or other navigation-related actions. These actions are independent nodes that communicate with the behavior tree (BT) through the action server.

2.2 Related Materials

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

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

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

3. Running Examples

3.1 Pre-use Notes

This lesson uses the Raspberry Pi as an example.

For Raspberry Pi and Jetson-Nano boards, you need to open a terminal on the host computer and enter the command to enter the Docker container. Once inside the Docker container, enter the commands mentioned in this lesson in the terminal. For instructions on entering the Docker container from the host computer, refer to [01. Robot Configuration and Operation Guide] -- [4.Enter Docker (For JETSON Nano and RPi 5)].

For RDKX5 and Orin boards, simply open a terminal and enter the commands mentioned in this lesson.

3.2 Single-Point Navigation

Note:

• For Raspberry Pi and Jetson-Nano boards, you need to enter the Docker container first. For RDKX5 and Orin controllers, this is not necessary.

  Enter the Docker container (see [Docker course] --- [4. Docker Startup Script] for steps).

  All the following commands must be executed within the same Docker container (see [Docker course] --- [3. Docker Submission and Multi-Terminal Access] for steps).

• This section requires at least one existing map. Refer to [5.Gmapping-SLAM mapping], [6.Cartographer-SLAM mapping], [7.slam_toolbox mapping] or any of the SLAM Mapping courses.

Robot car terminal launches the underlying sensor command:

xxxxxxxxxx
ros2 launch yahboomcar_nav laser_bringup_launch.py

Enter the command to start RViz visualization mapping (RViz visualization can be started on either the vehicle's infotainment system or a virtual machine; choose one, but do not start it on both the virtual machine and the vehicle's infotainment system simultaneously :). The image below shows the input on the vehicle's infotainment system.

xxxxxxxxxx
ros2 launch yahboomcar_nav display_nav_launch.py

At this point, the map is not yet loaded because the navigation program has not started. Next, run the navigation node by entering (you need to be in the same Docker terminal as above) in the terminal.

# Choose one of two navigation algorithms
# teb
ros2 launch yahboomcar_nav navigation_teb_launch.py 
# dwa
ros2 launch yahboomcar_nav navigation_dwa_launch.py 

At this point, you can see the map has loaded. Then, click on 【2D Pose Estimate】 to set the initial pose for the robot. Based on the robot's position in the actual environment, use the mouse to click and drag in RViz to move the robot model to the position you set. As shown in the image below, if the radar-scanned area roughly overlaps with the actual obstacles, the pose is accurate. After pose initialization, the robot model and the inflated region will appear in the RViz interface.

For single-point navigation, click the 【2D Goal Pose】 tool, then use the mouse to select a target point and orientation in RVIZ, then release.

The robot plans a path based on its surroundings and moves along it to the target point.

After the robot successfully reaches the target point, the vehicle terminal will display "Goal succeeded," indicating successful navigation.

4. View the Node Communication Graph

Enter the terminal:

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

If the graph does not display initially, select [Nodes/Topics (all)] and click the refresh button in the upper left corner. The original graph is too large; you can view it in the current lesson folder.

5. View the TF Tree

Enter the terminal:

xxxxxxxxxx
ros2 run rqt_tf_tree rqt_tf_tree

If the page doesn't display initially, click the refresh icon in the upper left corner to refresh the page. The original image is too large; you can view it in this lesson's folder.

6. Navigation Explanation

6.1 Core Steps of the Navigation Process

6.1.1 Phase 1: System Initialization and Map Loading

• Map Acquisition
• Load a pre-built raster map from map_server
• The map contains static obstacle information and serves as the basic environment model for navigation.

Cost Map Initialization

• Global Costmap: Based on the static map, used for global path planning.
• Local Costmap: Integrates the real-time scan topic of the lidar for dynamic obstacle avoidance.

6.1.2 Phase 2: Sensor Data Processing and Environmental Perception

Sensor Data Access

• LiDAR (/scan), IMU (/imu/data), and odometry (/odom) data are input via ROS topics.

Cost Map Update

• The local cost map updates dynamic obstacle information in real time. Each grid cell calculates an occupancy probability and cost based on sensor data (e.g., the closer to the obstacle, the higher the cost).
• Inflation: Expands the edges of obstacles to prevent the robot from getting too close.

6.1.3 Phase 3: Path Planning (Global and Local Planning)

Global Path Planning

• Input: Start point (current robot pose), end point (goal pose), and global cost map.
• The Dijkstra planner algorithm generates an optimal path (a series of discrete coordinate points) from the start point to the end point.

Local Path Planning and Tracking

The local planner, DWBLocalPlanner, generates short-term control commands that the robot can execute based on the global path and the local costmap.

6.1.4 Phase 4: Control Execution and Behavior Decision-Making

Controller Output

• The controller converts the local path into the robot's linear velocity (linear.x) and angular velocity (angular.z), which are published via the /cmd_vel topic.
• Velocity Smoothing: This prevents abrupt changes in robot motion and improves stability.

Behavior Tree Decision Process

The behavior tree defines the priority and state transition logic for navigation tasks. A typical process is as follows:

1. Checking if the target is reachable: If global planning fails, trigger a "recovery behavior" (e.g., replanning).
2. Performing Local Obstacle Avoidance: When an obstacle is detected in the local costmap, temporarily deviate from the global path.
3. Reaching the Target: When the error between the robot's pose and the target pose is less than a threshold, the task is completed.

Recovery Behavior Mechanism

When navigation encounters an anomaly (such as a completely blocked path), a pre-set recovery strategy (such as rotating to search for a new path or backing off to replan) is triggered.

6.2 Key Technical Principles

6.2.1 Cost Map

• Probabilistic Grid Representation: Each grid stores an occupancy probability (0-1), which is updated based on sensor data (such as ray detection from a LiDAR).
• Multi-layer Cost Overlay: Static cost (for obstacles in the map) + dynamic cost (for obstacles detected by real-time sensors) + inflation cost (for safety distance).

A cost map is a two-dimensional or three-dimensional map created and updated by the robot using sensor information. The figure below provides a brief overview.

In the image above, the red area represents obstacles in the costmap, the blue area represents obstacles expanded by the radius of the robot's inscribed circle, and the red polygon represents the footprint (the vertical projection of the robot's outline). To avoid collisions, the footprint should not intersect the red area, and the robot's center should not intersect the blue area.

The ROS costmap uses a grid format, with each cell value (cell cost) ranging from 0 to 255. It is divided into three states: occupied (obstacles present), free area (no obstacles), and unknown area.

The specific states and values ​​are shown in the following figure:

The image above can be divided into five sections, with the red polygon representing the robot's outline:

• Lethal: If the robot's center coincides with the center of this grid, the robot will inevitably collide with the obstacle.
• Inscribed: If the grid's circumcircle is inscribed within the robot's outline, the robot will inevitably collide with the obstacle. - Possibly circumscribed: The mesh's circumscribed circle circumscribes the robot's outline. In this case, the robot is essentially near an obstacle, so there's no collision.
• Freespace: Space without obstacles.
• Unknown: Space without known obstacles.

Global costmap:

Local costmap:

6.2.2 AMCL Localization Algorithm

AMCL (Adaptive Monte Carlo Localization) is a probabilistic localization system for 2D mobile robots. It implements an adaptive (or KLD sampling) Monte Carlo localization method that uses a particle filter to estimate the robot's position based on a given map.

As shown in the figure below, if the odometry is error-free, in a perfect world, we can directly use the odometry information (top half) to infer the robot's (base_frame) position relative to the odometry coordinate system. However, in reality, odometry drift and non-negligible cumulative errors exist. Therefore, AMCL uses the method in the bottom half. This method first uses the odometry information to preliminarily locate the base_frame. Then, using the measurement model, we determine the base_frame's position relative to the map_frame (global map coordinate system), thus determining the robot's position within the map. (Note that while the base-to-map transformation is estimated here, the final published transformation is the map-to-odom transformation, which can be understood as odometry drift.)

In the TF tree during robot navigation, the coordinate transformation between the map (map coordinate system) and the odom (odometry coordinate system) is published by the AMCL localization algorithm, while the coordinate transformation between the odom (odometry coordinate system) and the besel_footprint (robot chassis center projection coordinate system) is published by the ekf_filter_node node in the robot_localization package. Its function is to publish odometry information after fusion of the raw odometry from the IMU sensor and encoder.

In the node communication diagram above, we can see the communication data flow for the ekf_filter_node.

The /driver_node node is the robot chassis node, publishing /imu/data_raw (raw IMU sensor data) and /odom_raw (raw encoder odometry data).

The imu_filter node subscribes to / imu/data_raw (raw IMU sensor data). The imu_filter node filters the raw IMU sensor data and publishes it to the /imu/data topic.

The ekf_filter_node node subscribes to the /odom and /imu/data topics, fuses the multi-sensor data, and publishes it to the /odom topic.

6.3 Path Planning

6.3.1 Global Path Planning

Global path planning calculates an optimal or feasible global path from a start point to a destination within a known environment map, disregarding real-time obstacles. The path from the start point to the destination is calculated with the global optimum as the goal. As shown in the figure below, the path connected by arrows is the global path.

6.3.2 Local Path Planning

Local path planning dynamically adjusts the robot's path based on real-time LiDAR sensor data during movement to avoid obstacles (obstacles are displayed in the costmap). As shown in the figure below, the blue path is the local path.

6.4 Navigation Parameter Configuration

Jetson Nano and Raspberry Pi require Docker first

Configuration file path:

x
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/teb_nav_params.yaml
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/dwa_nav_params.yaml

The default configuration parameters for the TEB algorithm navigation function are as follows:

xxxxxxxxxx
amcl:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间 Use simulation time
    alpha1: 0.1          # 里程计旋转误差模型参数 Odometry rotation error model parameters
    alpha2: 0.1          # 里程计平移误差模型参数 Odometry translation error model parameters
    alpha3: 0.3          # 里程计平移误差模型参数 Odometry translation error model parameters
    alpha4: 0.3          # 里程计旋转误差模型参数 Odometry rotation error model parameters
    alpha5: 0.1          # 里程计旋转误差模型参数 Odometry rotation error model parameters
    base_frame_id: "base_footprint"  # 机器人基座坐标系 Robot base coordinate system
    beam_skip_distance: 0.5          # 光束跳过距离阈值 Beam skip distance threshold
    beam_skip_error_threshold: 0.9   # 光束跳过误差阈值 Beam skip error threshold
    beam_skip_threshold: 0.3         # 光束跳过阈值 Beam skip threshold
    do_beamskip: false               # 是否启用光束跳过 Enable beam skipping
    global_frame_id: "map"            # 全局坐标系 Global coordinate system
    lambda_short: 0.1                # 激光模型短距离权重 Laser model short distance weight
    laser_likelihood_max_dist: 2.0   # 激光似然场最大距离 Maximum distance of the laser likelihood field
    laser_max_range: 100.0           # 激光最大有效距离 Maximum effective laser range
    laser_min_range: -1.0            # 激光最小有效距离 Minimum effective laser range
    laser_model_type: "likelihood_field"  # 激光模型类型（似然场） Laser model type (likelihood field)
    max_beams: 50                    # 每次处理的最大激光束数 Maximum number of laser beams processed per shot
    max_particles: 4000              # 最大粒子数 Maximum number of particles
    min_particles: 1000              # 最小粒子数 Minimum number of particles
    odom_frame_id: "odom"            # 里程计坐标系 Odometry frame
    pf_err: 0.05                     # 粒子滤波器误差阈值 Particle filter error threshold
    pf_z: 0.99                        # 粒子滤波器权重指数 Particle filter weight exponent
    recovery_alpha_fast: 0.0          # 快速恢复参数 Fast recovery parameter
    recovery_alpha_slow: 0.0          # 慢速恢复参数 Slow recovery parameter
    resample_interval: 1             # 重采样间隔 Resampling interval
    robot_model_type: "nav2_amcl::DifferentialMotionModel"  # 机器人运动模型 Robot motion model
    save_pose_rate: 0.5              # 保存位姿的速率 Save pose rate 
    sigma_hit: 0.2                   # 激光模型标准差 Laser model standard deviation
    tf_broadcast: true               # 是否广播TF变换 Broadcast TF transforms
    transform_tolerance: 1.0          # TF变换容差 TF transform tolerance
    update_min_a: 0.15               # 最小角度变化触发更新 Minimum angle change to trigger an update
    update_min_d: 0.15               # 最小距离变化触发更新 Minimum distance change to trigger an update
    z_hit: 0.5                       # 激光模型命中权重 Laser model hit weight
    z_max: 0.05                      # 激光模型最大距离权重 Laser model maximum distance weight
    z_rand: 0.5                      # 激光模型随机权重 Laser model random weight
    z_short: 0.05                    # 激光模型短距离权重 Laser model short distance weight
    scan_topic: scan                 # 激光扫描话题 Laser scan topic
​
amcl_map_client:
  ros__parameters:
    use_sim_time
 slam_toolbox mapping
Navigation2 multi-point navigation avoid