8、Navigation and obstacle avoidance

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

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

Navigation2 corresponds to the paper: https://arxiv.org/pdf/2003.00368.pdf

teb_local_planner：https://github.com/rst-tu-dortmund/teb_local_planner/tree/foxy-devel

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

The operating environment and software and hardware reference configurations are as follows:

• REFERENCE MODEL: ROSMASTER R2

• Robot hardware configuration: Arm series main control, Silan A1 lidar, AstraPro Plus depth camera

• Robot system: Ubuntu (version not required) + docker (version 20.10.21 and above)

• PC Virtual Machine: Ubuntu（22.04）+ ROS2（Humble）

• Usage scenario: Use on a relatively clean 2D plane

8.1、Brief introduction

Navigation2 overall architecture diagram

Navigation2 has the following tools:

● Tools for loading, serving, and storing maps (Map Server)

● Tool to locate the robot on the map (AMCL)

● Navigate from point A to point B with Nav2 Planner

● Tool to control the robot during the following path (Nav2 Controller)

● Tools for converting sensor data into cost map representations in the world of robotics (Nav2 Costmap 2D)

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

● Tool to calculate recovery behavior in the event of a failure (Nav2 Recoveries)

● Nav2 Waypoint Follower

● Tools and watchdog for managing server lifecycles (Nav2 Lifecycle Manager)

● Plugins that enable user-defined algorithms and behaviors (Nav2 Core)

Navigation 2 (Nav 2) is the navigation framework that comes with ROS 2 and aims to move mobile robots from point A to point B in a safe way. As a result, Nav 2 can perform dynamic path planning, calculate motor speed, avoid obstacles, and restore structures.

Nav 2 uses Behavior Trees (BT) to call modular servers to complete an action. Actions can be calculated paths, control efforts, recovery, or other navigation-related actions. These actions are independent nodes that communicate with the Behavior Tree (BT) through the Action Server.

8.2、use

8.2.1、pre-use configuration

Note: Since ROSMASTER series robots are divided into multiple robots and multiple devices, the factory system has been configured with routines for multiple devices, but because the product cannot be automatically identified, it is necessary to manually set the machine type and radar model.

Raspberry Pi PI5 master control needs to enter the docker container first, Orin master control does not need to enter,

according to the model of the car, the type of radar and the type of camera, make the following modifications:

root@ubuntu:/# cd
root@ubuntu:~# vim .bashrc

After the modification is complete, save and exit vim, and then execute:

xxxxxxxxxx
root@ubuntu:~# source .bashrc
--------------------------------------------------------
ROS_DOMAIN_ID: 12
my_robot_type: x3 | my_lidar: a1 | my_camera: astraplus
--------------------------------------------------------
root@ubuntu:~#

You can see the model of the currently modified car, the type of radar and the type of camera

8.2.2、start chassis and radar-related nodes

First of all, you need to do port binding operations in the host [that is, on the jetson of the car] [see the port binding tutorial chapter], which mainly uses two devices: radar and serial port;

Then check whether the radar and serial device are in the port binding state: on the host [that is, the jetson of the car], refer to the following command to view, and the successful binding is the following state:

If the radar or serial device is not bound to the display, you can plug and unplug the USB cable to view it again

Raspberry Pi PI5 master needs to enter the docker container first, Orin master does not need to enter,

Enter the docker container (for steps, please refer to [docker course chapter ----- 5. Enter the robot's docker container]),

and execute it in a terminal:

xxxxxxxxxx
ros2 launch yahboomcar_nav laser_bringup_launch.py

8.2.3、Start rviz display map

Ubuntu virtual machine and docker container are configured with multi-machine communication, this step is recommended to start in the virtual machine: to keep time synchronized and reduce resource consumption, because if you use VNC, you are very network-dependent, which may lead to navigation failure.

[Note, here must first start the node that displays the map, and then start the navigation node in step 3, this is because the navigation2 terminal map topic is only published once, how to start the navigation node first, and then start the rviz display, it is possible that you may not be able to subscribe to the only map topic published, resulting in the map not being displayed]

xxxxxxxxxx
ros2 launch yahboomcar_nav display_nav_launch.py

At this time, the map will not be displayed in the screen, and the topics of each node on the left do not need to be red, because the navigation node has not been started.

8.2.4、Start the navigation node

• There are two navigation algorithms: DWA and TEB

• Navigation can be divided into single-point navigation and multi-point navigation, which are described below.

1. Start the navigation node, enter the docker container, and execute it in terminals:

xxxxxxxxxx
ros2 launch yahboomcar_nav navigation_dwa_launch.py   #DWA navigation
or
ros2 launch yahboomcar_nav navigation_teb_launch.py   # TEB navigation

2. Click [2D Pose Estimate] on rviz, and then compare the position of the car to mark the initial position of the car on the map;

The display after marking is as follows:

3. Compare the overlap between the radar scanning point and the obstacle, and you can set the initial posture of the trolley many times until the radar scanning point and the obstacle roughly coincide;

8.2.5、Single point navigation

After the initial posture is set, you can click [2D Goal Pose] to set a navigation target point, and the car will start single-point navigation;

8.2.6、multi-point navigation

1、After the initial posture is set, you can click [Panels] in the upper left corner of rviz --- Select [Navigation 2], and the [Navigation 2] panel will be displayed

2、Click [Waypoint mode] in the figure above, and then click [Navigation2 Goal] on rviz to mark a target point in the map, and click [Navigation2 Goal] again to mark the second target point in the map, and if the cycle continues, you can mark multiple target points at once;

3、After marking multiple target points, click [Start Navigation] to start multi-point navigation;

4、After the multi-point navigation is completed, the car will stay in the position of the last target point;

5、This may occur during navigation due to the ros-foxy version of navigation2 itself, which has been fixed in subsequent ros2 releases

8.3、node resolution

8.3.1、display the computational graph

xxxxxxxxxx
rqt_graph

8.3.2、Navigate the details of each node

/amcl

/bt_navigator

/controller_server

/global_costmap/global_costmap

/lifecycle_manager_localization

/local_costmap/local_costmap

/map_server

/planner_server

/recoveries_server

/waypoint_follower

8.3.3、TF transform

xxxxxxxxxx
ros2 run tf2_tools view_frames.py

8.4、Navigation2 details

• Comparison of navigation frames in ROS 1 and ROS 2

In Nav 2, move_base is split into multiple components. Unlike a single state machine (ROS1), Nav 2 separates ideas by leveraging the low-latency, reliable communication of the Action Server and ROS 2. The Behavior Tree is used to orchestrate and organize these tasks, which allows Nav 2 to have highly configurable navigation behavior through the Behavior Tree XML file without the need to program the task.

nav2_bt_navigator replaces the move_base at the top level, which uses an Action interface to complete the navigation tasks of a tree-based action model. Implement more complex state machines via BT and add recovery behavior as additional Action Servers. These behavior trees are configurable XML.

The planning, recovery, and controller servers are also action servers that the BT Navigator can call for calculations. All 3 servers can host many plugins for many algorithms, each of which can individually invoke specific behaviors from the navigation behavior tree. The default plugins provided are ported from ROS 1, namely: DWB, NavFn and similar recovery behaviors such as rotating and clearing costmaps, plus a new recovery mechanism that waits for a fixed time. These servers are called from the BT Navigator through their action servers to calculate results or complete tasks. The state is maintained by the BT Navigator behavior tree. All these changes make it possible to replace these nodes at startup/runtime by implementing any algorithm of the same interface.

Porting kit:

• AMCL: Ported to nav2_amcl

• map_server: Transplant to nav2_map_server

• nav2_planner: Replace global_planner and manage 'N' planner plugins

• nav2_controlle: Replace local_planner and manage 'N' controller plugins

• Navfn: Ported to nav2_navfn_planner

• DWB: Replaces DWA and ported to the nav2_dwb_controller metapackage in ROS 2

• nav_core: Ported to nav2_core and updated interfaces

• costmap_2d: Transplant to nav2_costmap_2d

New Package:

• Nav2_bt_navigator: Replace move_base state machine

• nav2_lifecycle_manager: Handles the life cycle of the server program

• nav2_waypoint_follower: Complex tasks can be performed through many waypoints

• nav2_system_tests: A set of basic tutorials in integration testing and simulation for CI

• nav2_rviz_plugins: An rviz plugin that controls the Navigation2 server, commands, cancellation, and navigation

• nav2_experimental: Experimental (and incomplete) work of deep reinforcement learning controllers

• navigation2_behavior_trees: A wrapper for calling the behavior tree library of the ROS action server

8.4.1、amcl

The full name of amcl in English is adaptive Monte Carlo localization, which is a probabilistic positioning system for two-dimensional mobile robots. It's an upgraded version of the Monte Carlo positioning method, using an adaptive KLD method to update particles and a particle filter to track the robot's pose based on a known map. As currently implemented, this node is only suitable for laser scanning and laser maps. It can be extended to handle other sensor data. AMCL receives laser-based maps, laser scans, and transformation information, and outputs pose estimates. On startup, AMCL initializes its particle filter based on the provided parameters. Note that due to the default settings, if no parameters are set, the initial filter state will be a medium-sized particle cloud centered on (0,0,0).

Monte Carlo

• Monte Carlo method, also known as statistical simulation method, statistical test method, is an idea or method. It is a numerical simulation method that uses probability phenomena as research objects. It is a calculation method that estimates unknown characteristic quantities by obtaining statistical values according to the sampling survey method.

• For example, if there is an irregular shape in a rectangle, how to calculate the area of the irregular shape? It's not easy to count. But we can approximate. Take a bunch of beans, sprinkle them evenly on the rectangle, and then count the number of beans in the irregular shape and the number of beans in the remaining places. The rectangular area is known, so the irregularly shaped area is obtained by estimating. In terms of robot positioning, it is possible that it is located in any location on the map, how can we express the confidence of a location in this case? We also use particles, where there are more particles, it means that the probability of where the robot is is high.

The greatest advantage of Monte Carlo

• The error of the method is independent of the dimensionality of the problem.

• Problems of a statistical nature can be solved directly.

• Discretization is not necessary for continuity problems

Disadvantages of Monte Carlo Law

• For deterministic problems, it needs to be converted into random problems.

• Error is probability error.

• Usually requires more counting steps N.

Particle filtering

• The number of particles represents the likelihood of something. Through some kind of evaluation method (evaluating the likelihood of this thing), the distribution of particles is changed. For example, in robot positioning, a certain particle A, I think the probability of this particle in this coordinate (for example, this coordinate belongs to the "this thing" mentioned earlier), then I give him a high score. The next time you reposition all the particles, arrange more near this position. After a few more rounds in this way, the particles will all concentrate in the position with high probability.

Adaptive Monte Carlo

• Solved the robot kidnapping problem that will re-scatter some particles globally when it finds that the average score of the particles has suddenly decreased (meaning that the correct particle was discarded in an iteration).

• Solved the problem of fixed particle number, because sometimes when the robot positioning is almost obtained, such as these particles are concentrated together, there is no need to maintain so many particles, this time the number of particles can be less.

8.4.2、costmaps and layers

The current environmental representation is a cost map. A cost map is a grid of regular 2D cells containing cells from unknown, idle, occupied, or bloated costs. This cost map is then looked for to calculate global plans or sampling to calculate local control efforts. Various cost map layers are implemented as pluginlib plugins to buffer information into cost maps. This includes information from LIDAR, RADAR, sonar, depth sensors, image sensors, and more. It is best to process sensor data before it is entered into the layer map, but it is up to the developer. You can use a camera or depth sensor to create a cost map layer to detect and track obstacles in your scene to avoid collisions. In addition, layers can be created to vary the underlying cost plot based on some rules or heuristics. Finally, they can be used to buffer real-time data into the 2D or 3D world for binarized labeling of obstacles.

8.4.3、planner_server

The task of the planner is to calculate the path based on a specific objective function. Depending on the nomenclature and algorithm chosen, a path is also called an itinerary route. There are two typical examples, one is to calculate a path plan to the target location (e.g. from the current location to a target pose) or completely override the complete coverage (e.g. a path plan that covers all free space). The planner has access to cached data for global environment representations and sensors. The planner can have the following features:

• Calculates the shortest path

• Calculate the full coverage path

• Calculates routes along sparse or predefined routes

The general task of the planner in Nav2 is to calculate an efficient and possibly optimal path from the current position to the target pose.

The algorithm plug-in in Planning Server uses the environment information captured by different sensors to search for the path of the bot. Some of these algorithms obtain paths by searching for environment rasters, while others extend the possible states of the robot by considering the feasibility of the paths.

As mentioned earlier, Planning Server uses plugins that work on grid spaces, such as NavFn Planner, Smac Planner 2D, and Theta Star Planner. NavFn planner is a navigation function planner that uses the Dijkstra or A algorithm. Smac 2D planner implements a 2D A algorithm with 4 or 8 neighbors connected and with smoother and multi-resolution characteristics. Theta Star planner (smooth arbitrari-angle path planning in a continuous environment, Theta is a variant of A, it propagates information along the edges of the graph, but does not constrain the path to the edges of the graph (looking for paths at "arbitrary angles))) is achieved using a line-of-sight algorithm by creating non-discrete directional path segments. Commonly used planners are as follows:

8.4.4、controller_server

The controller, also known as the local planner in ROS 1, completes the following of the global calculation path or completes a local task. The controller has access to the local environment representation and attempts to calculate a possible path for the reference path to follow. Many controllers project the robot forward into space and calculate locally feasible paths with each update iteration. The controller has the following functions:

• Follow a path

• Use the detector to complete the docking with the charging pile in the mileage coordinate system

• Board an elevator

• Interface with a tool

In Nav2, the general task of the controller is to calculate an effective control effort to follow the global planning path. However, there are many controller classes and local planner classes. The goal of the Nav2 project is that all controller algorithms can be used as plug-ins in this server for general research and industrial tasks.

Common controller plug-ins are DWA Controller and TEB Controller

1、DWA Controller The idea of DWA algorithm is to sample different possible speeds, and then simulate different trajectories, use the distance to the target point, obstacle distance and time to evaluate, select the trajectory with the best score, and issue speed commands to control the trolley forward. The speed sampling simulated by the DWA algorithm is not randomly selected, but there is a range, the size of the speed is limited by the maximum speed and minimum speed of the trolley, the size of the acceleration is affected by the performance of the trolley motor, in addition, in order to ensure safety, the distance of the obstacle will also affect the value of the speed.

Differential trolley, wheat wheel car in the adjacent time, the movement distance is short, can see the motion trajectory between the adjacent two points, as a straight line, the motion trajectory of other models as an arc.

Three evaluation functions:

• heading(v, w): azimuth evaluation function, the angle difference between the car and the target, the smaller the angle difference, the higher the score

• dist(v, w): The distance between the trolley and the nearest obstacle, the farther the distance, the higher the score

• velocity (v, w): The speed corresponding to the trajectory, the higher the speed, the higher the score

Physical meaning: make the car towards the target point, avoid obstacles, and drive quickly

2、TEB Controller The idea of TEB algorithm is to regard the path connecting the starting point and the end point as similar to a rubber band that can be deformed, and then treat the external constraint as an external force to deform the path.

• Follow Path + Obstacle Avoidance: The constraint has two main goals, following a consistent global path planning and obstacle avoidance. Both objective functions are very similar. Follow the path to apply external force to pull the local path to the global path, and the obstacle avoidance constraint force makes the local path away from the obstacle

• Velocity/acceleration constraints: Speed and acceleration should be within a certain range

• Kinematic constraints: a smooth trajectory composed of several arcs, the control quantity is only linear and angular velocity, the Ackermann structure has a minimum turning radius, and the wheat wheel/omnidirectional/differential speed is 0

• Fastest Path Constraint: The objective function allows the robot to obtain the fastest path, and the poses on the path are evenly separated in time, instead of the shortest path in traditional space

TEB can be formulated as a multi-objective optimization problem, most of which are local and related to only a small set of parameters, since they depend on only a few consecutive robot states. After determining the location of N control points, the path is optimized using the open source framework g2o (General Graph Optimization) general graph optimization method. In the case of the trolley, all pose points, time intervals, obstacles, etc. are described as points, and constraints are described as edges. The minimum distance from an obstacle connects the obstacle to the pose point, and the velocity constraint connects two adjacent pose points with their time difference. In general, the local trajectory generated by TEB is composed of a series of discrete poses with time information, and the goal optimized by the g2o algorithm is that the trajectory composed of these discrete poses can finally reach the shortest time, shortest distance, away from obstacles and other goals, while limiting the speed and acceleration to make the trajectory meet the robot dynamics.

8.4.5、recoveries_server

Recovery Behaviors are the core module of the fault-tolerant system in robot navigation. Their goal is to autonomously handle abnormal states or environmental changes during navigation through predefined behavior logic. The Humble version of behavior_server is based on the Behavior Tree architecture, encapsulating recovery behaviors as pluggable nodes, supporting dynamic combination and priority scheduling, thereby achieving a more flexible fault recovery strategy. Examples include sensor failures in the perception system that cause the environmental representation (such as the cost map) to be filled with false obstacles. Such failures trigger the ClearCostmap node in the behavior tree, calling the service to clear the obstacle layer of the local or global cost map, and restore the accuracy of the environmental representation to allow the robot to continue moving. Another example is that the robot is stuck due to dynamic obstacle blocking, path planner failure, or control error accumulation. At this point, the behavior tree will execute a recovery sequence: first, the BackUp node is used to control the robot to move a specified distance (such as 0.5 meters) in the opposite direction along the trajectory. If it still cannot escape, the Spin node is triggered to rotate the robot in place by a specific angle (such as 180°) to reset the local planning state and try to enter the free space. Finally, when multiple recovery failures or hardware-level failures (such as motor communication interruption and navigation stack crash) are detected, the behavior tree will execute a safe recovery strategy: send an alarm notification to the operator through the NotifyOperator node (integrated with Slack, Telegram, or email services), and call the EmergencyStop node to forcefully stop all motion controllers and switch to safe standby mode to wait for human intervention.

8.4.6、waypoint following

Waypoint following is one of the basic functions of a navigation system. It tells the system how to use the navigator to reach multiple destinations. The nav2_waypoint_follower package contains a waypoint tracker that has a plugin interface for specific executors. This is useful if you need to have the robot travel to position and complete specific tasks like taking a picture, picking up a box, or waiting for user input. This is a nice demo application to show how to use Nav2 in a sample application.

However, the package can be used not only for sample applications. There are two schools of thought about robot fleet manager/scheduler: dumb robot + intelligent centralized scheduler; Smart robot + dumb centralized scheduler.

In the first idea, nav2_waypoint_follower software packages are sufficient to create a production-level robotic solution. Since autonomous systems/schedulers take into account factors such as the robot's posture, battery level, current task, etc. when assigning tasks, applications on robots only need to care about the task at hand and not with other complexities of completing the task that the system requires. In this case, a request sent to a waypoint follower should be treated as 1 unit of work (e.g., 1 pick in the warehouse, 1 safety patrol loop, 1 aisle, etc.) to perform the task, and then return to the scheduler for the next task or request charging. In this school of thought, waypoint following applications are just a step above the navigation software stack and below the system's autonomous applications.

In the second thought, the nav2_waypoint_follower package is a nice example application/proof of concept, but it does require a waypoint tracking/autonomous system on a robot to take on more tasks to develop a robust solution. In this case, you should use the nav2_behavior_tree package to create a custom application-level behavior tree to use navigation to complete the task. This can contain subtrees, such as checking the charging status in a task to return to the dock, or handling more than 1 unit of work in a more complex task. Soon, there will be an nav2_bt_waypoint_follower (name to be adjusted) that will make it easier for users to create this application. In this school of thought, waypoint following applications are more closely tied to autonomous systems, or in many cases, waypoint following applications are the autonomous system itself.

These two schools of thought can't simply say who is better than whom, and who is better depends largely on what task the bot is accomplishing, what type of environment it is in, and what cloud resources are available. Often, for a given business case, this distinction is very obvious.

8.4.7、bt_navigator

The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose task interface. It is a behavior tree-based navigation implementation designed to allow flexibility in navigation tasks and provide a way to easily specify complex bot behaviors, including recovery.

Planer Server, Controller Server, and Recovery Server each implement their respective functions, namely global path planning, local path planning, and recovery operations. But their functions are independent and do not interfere with each other. To achieve a complete navigation function, the cooperation of various modules is required. BT Navigator Server was the one who assembled the Lego bricks.

For complete parameter configuration, see:

xxxxxxxxxx
/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/dwa_nav_params.yaml
/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/teb_nav_params.yaml