Navigation in ROS2

Map building in ROS2

A map is a picture of the area in which the robot is working. A map is used by the robot to plan and localize its trajectories. An occupancy grid map is what a map is in ROS. In plain English, each map cell represents an obstacle by having a certain value. We require a robot with LIDAR, odometry, and the robot environment in order to create a map.

SLAM Simultaneous Localization and Mapping (SLAM) will locate the robot while also making a map. SLAM algorithms are used to make maps of uncharted environments while simultaneously being aware of where the robot is. Cartographer and SLAM-Toolbox are two excellent SLAMs for ROS2.

Cartographer

Cartographer (https://opensource.google/projects/cartographer) is a system that offers real-time simultaneous localization and mapping (SLAM) in 2D and 3D across numerous platforms and sensor configurations. Google creates cartographer, and they have been mapping building interiors using Google Street View.

Cartographer ros is a ROS wrapper for cartographer that enables integration with ROS (https://google-cartographer-ros.readthedocs.io/en/latest/). This is fantastic since you can utilize only the pre-built packages and precisely configure them for your robot. In this unit, this procedure is detailed.

This section will describe the robot's launch file for the cartographer. The primary advantage of a launch file is the ability to launch many nodes from a single file and set a node-specific parameter. The parameters may be supplied in the launch file or loaded from a YAML file. We must launch two nodes in order to launch "cartographer."

1. Launch of the cartographer_node

• The cartographer_node is provided by the package cartographer_ros
• The executable is called cartographer_node
• The parameters it requires are: 1. use_sim_time : is a boolean indicating whether the node must synchronize its time with the simulation

The arguments are: 1. configuration_directory : the directory where to find the configuration files 2. configuration_basename : the config file name

 package='cartographer_ros'
 executable='cartographer_node',
 name='cartographer_node',
 output='screen', 
 parameters=[{'use_sim_time': True}],
 arguments=['-configuration_directory', cartographer_config_dir, 
  '-configuration_basename', configuration_basename]

• Create a new package in ros2_ws workspace named cartographer_slam inside the src/ directory.
• Create launch & config directories at ros2_ws/src/cartographer_slam
• Write a launch file to launch cartographer with the name cartographer.launch.py where the two nodes are launched
• Create an LUA file named cartographer.lua in the config directory with the following config parameters:
• Compile everything
• Launch your program
• code
• ros2 launch cartographer_slam cartographer.launch.py
• Launch RVIZ to see the map being created. You will configure RVIZ to show the data you want to control
• Add the map display in RVIZ and configure it to see the map you are generating

Fig. Map created from Cartographer SLAM

You must launch an executable named map saver, which launches a map saver node from nav2 map server, in order to save the map you've made. When saving the map, call the node inside the directory.

 cd ~/ros2_ws/src/cartographer_slam/config
 ros2 run nav2_map_server map_saver_cli -f turtlebot_area

Note:: Never invoke the map saver before closing the Cartographer node. If not, you will lose the map you made.

The command to save will produce two files: The map is represented by the occupancy grid image in the name.pgm file. The name.yaml file gives information about the map's resolution.
Example of YAML File of the Map.

 image: turtlebot_area.pgm
 mode: trinary
 resolution: 0.05
 origin: [-5.9, -5.22, 0] 
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.25

image: Name of the file containing the image of the generated map.
resolution: Resolution of the map (in meters/pixel).
origin: Coordinates of the lower-left pixel on the map. These coordinates are given in 2D (x, y, z). The third value indicates rotation. If there is no rotation, the value will be 0.
occupied_thresh: Pixels with a value greater than this value will be considered an occupied zone (marked as an obstacle)
free_thresh: Pixels with a value smaller than this will be considered a completely free zone.
negate: Inverts the colors of the map. By default, white means completely free, and black means completely occupied.

Map_server
Nav2_lifecycle_manager

Data required to launch each node:-

1. For the launch of the map_server node

These are the fields you need to indicate in the node launch:

The map_server is provided by the package nav2_map_server
The executable is called map_server
The parameters it requires are:

1. use_sim_time : is a boolean indicating if the map_server must synchronize its time with the simulation.
2. yaml_filename : is the complete path to the map yaml file.
   

 package='nav2_map_server',
 executable='map_server', 
 name='map_server',
 output='screen', 
 parameters=[{'use_sim_time': True}, 
             {'yaml_filename':map_file}
            ]),

1. For the launch of the lifecycle_manager node
   

• This node manages the lifecycle of nodes involved in navigation
  
• The lifecycle_manager is provided by package nav2_lifecycle_manager.
  
• The executable is called lifecycle_manager.
  
• Parameters required:

1. use_sim_time : is a boolean indicating whether the map_server must synchronize its time with the simulation
2. autostart : is a boolean that indicates if the lifecycle manager has to start when launched.
3. node_names : is a list with the names of the nodes that the lifecycle manager has to take care of. So far, only the map_server

 package='nav2_lifecycle_manager',
 executable='lifecycle_manager',
 name='lifecycle_manager_mapper', 
 output='screen',
 parameters=[{'use_sim_time': True}, 
             {'autostart': True}, 
             {'node_names': ['map_server']}])

Steps for creating a map_server launch file:

• Create a map_server package in ros2_ws
• Create a launch directory inside that package, with a launch file named nav2_map_server.launch.py
• In the launch file, include the launch of the two nodes required to have a map server running:

1. Map_server
2. Nav2_lifecycle_manager

• Create a config directory inside that package and put inside it the map files you generated.
• Launch the map server loading the map in the config directory, and visualize the map in RVIZ.

Localization of a Robot in an Environment

Knowing one's location in relation to their surroundings is referred to as localization. In your situation, the robot needs to be aware of its location and orientation in relation to its surroundings, i.e. the world map that was made in the previous unit. This is referred to as a robot's "position." AMCL(Adaptive Monte-Carlo Localization)., an extremely reliable localization algorithm in ROS. For a robot moving in two dimensions, t is a probabilistic localization mechanism. It employs Dieter Fox's adaptive (or KLD-sampling) Monte Carlo localization method, which uses a particle filter to compare a robot's pose to a predetermined map. When someone publishes a transform between the /map frame and the /odom frame, a ROS robot is located. This implies that the robot's /odom frame is aware of its position in relation to the /map frame. As a result of its direct connection to the /odom frame through its /base link frame, the robot is aware of its location on the map. When everything is in order, that transform is published by the AMCL. We'll examine how to set up AMCL to publish that transformation.

How to do localization in ROS2

You need to launch three nodes:

• The map server provides the map to the localization algorithm
• The localization algorithm
• The life cycle manager

1. Launch of the map_server node

These are the fields you need to indicate in the node launch:

• The map_server is provided by the package nav2_map_server
• The executable is called map_server
• The parameters it requires are:

1. use_sim_time : is a boole
2. use_sim_time : is a boolean indicating if the map_server must synchronize its time with the simulation.
3. yaml_filename : is the complete path to the map yaml file

 package='nav2_map_server',
 executable='map_server',
 name='map_server',
 output='screen',
         parameters=[{'use_sim_time': True},
         {'yaml_filename':map_file}
        ]),

2. Launch of the amcl node

• These are the fields you need to indicate in the node launch:
• The amcl is provided by the package nav2_amcl
• The executable is called amcl
• The parameter it requires is:

1. the yaml file that contains all the configuration parameters of the node

 package='nav2_amcl',
 executable='amcl', 
 name='amcl', 
 output='screen',
 parameters=[nav2_yaml]

3. For the launch of the lifecycle_manager node

This node manages the lifecycle of nodes involved in navigation.

• The lifecycle_manager is provided by package nav2_lifecycle_manager .
• The executable is called lifecycle_manager .
• Parameters required:

1. use_sim_time : is a boolean indicating whether the map_server must synchronize its time with the simulation.
2. autostart : is a boolean that indicates whether the lifecycle manager has to start when launched.
3. node_names : is a list with the names of the nodes that the lifecycle manager has to take care of.

 package='nav2_lifecycle_manager',
 executable='lifecycle_manager',
 name='lifecycle_manager_localization',
 output='screen', 
 parameters=[{'use_sim_time': True}, 	
           {'autostart': True},
           {'node_names': ['map_server','amcl']}])

Create a launch file that launches the localization system for the robot using the map created in the previous section.

1. Create a new package named localization_server .
2. Create a config and a launch directory.
3. Inside the launch directory, include a launch file named localization.launch.py that launches the three nodes required for localization.
4. Inside the config directory, include a yaml file named amcl_config.yaml

Then you need to compile the package and launch the localization system. If the localization system has been properly launched, you should see on the screen a message like this:

  [amcl-2] [WARN] [1632242660.692356589] [amcl]: ACML cannot publish a pose or update the transform. Please set the initial pose…

This message is valid. The localization system is awaiting the robot's first pose. As a result, once the localization system is launched, the algorithm is unable to determine where to first locate the robot on a map. To begin tracking the robot as it moves, the algorithm needs someone to signal that it has done so.

To provide the initial localization manually, follow the next steps:

1. Launch RVIZ to visualize and set the following in RVIZ:

1. map , to show the current map being used
2. Robot Model , to show the robot structure. You will need to specify the topic that publishes the URDF model of the Turtlebot3 Burger robot.
3. On the Description Topic field, write /robot_description . That topic is published by the robot_state_publisher node which is launched with the simulation.
4. Laser Scan , to show how good the robot position estimation matches with the actual map
5. Pose With Covariance , to show the status of the localization
6. Particle Cloud , to see the (localization) particle distribution (Note: this display does not appear by topic ).

Set the robot's starting position on the map. To locate the robot in the environment, click on the 2D pose estimate. Then, click on the map point that represents the robot's actual position in the simulation. The entire AMCL system will start localizing the robot after the Pose Estimate is set. It will then begin publishing the transform between the map and the odom. The system as a whole goes live!

Localization of the robot in RVIZ

Save this RVIZ configuration in the ros2_ws/src directory as localizer_rviz_confi.

About robot localization

The robot is localized when you know its (x, y, ) in the corresponding map (for 2D localization):

• x is the x position of the robot in the map frame
• y is the y position of the robot in the map frame
• θ is the orientation of the robot in the map frame

The Adaptive Monte Carlo Localization method is used by the AMCL package to compute the (x, y) of the robot after using the static map. Particles are used in the Adaptive Monte Carlo method to locate the robot. Like the actual robot, these particles have their own coordinates, orientation values, and assigned weight. The absolute difference between the robot's actual stance and the pose that particular particle anticipated is what is referred to as the weight value (). The weight of the particle specifies the stance of the robot more precisely as it increases in size. Every time the robot moves across the environment and provides fresh sensor data, the particles are re-sampled. The particles with high weights survive each resample whereas the particles with low weights disappear. The particles will converge and evaluate an estimate of the robot's stance following numerous iterations of the AMCL algorithm. As a result, depending on the information from the sensor, this algorithm calculates the robot's orientation and location. You require an AMCL node that has been properly setup if you want to locate the robot accurately in its surroundings. The settings for tuning AMCL to customize the algorithm for your particular situation are listed below.

Path Planning in ROS2

Planning a path from Point A to Point B while avoiding potential pitfalls is known as path planning. You will launch numerous nodes in order to start Path Planning. Additionally, certain additional nodes are needed for path planning. REQUIREMENTS

Prior to launching the path planning system, you need to run the following nodes:

1. nav2_map_server node
2. nav2_amcl node

To launch path planning, You need to launch four more nodes:

• planner node
• controller node
• manager of recovery behaviors node
• behavior tree navigator node
• nav2_lifecycle_manager node to manage the nodes.

1. Launch of the planner

The Global Planner in ROS1 is the same as the ROS Nav2 planner. Its job is to determine a route from point A to point B for the robot. The calculated route avoids the known barriers marked on the map. The robot immediately builds the path computation after receiving a 2d Goal Pose. The planner can also access a representation of the entire environment and sensor data that has been buffered into it (i.e., Global Costmap ).

The fields you need to indicate in the node launch are:

• The package nav2_planner provides the controller
• The executable is called planner_server
• The parameters it requires are:
• The yaml file that contains all the configuration parameters of the node

2. Launch of the controller

The Local Planner in ROS1 is the same as the ROS Nav2 controller. Its primary duty is to plan a reactive path from the current location to a few meters in the future (until the range of the sensors). Then, while attempting to adhere to the overall plan, it constructs a route to avoid the dynamic impediments (which do not appear on the map but can be spotted with the help of sensor data). Additionally, it is responsible for creating the wheel's orders, which direct the robot to follow the trajectory. There are only two local planners accessible in ROS2 at the moment:

• dwb_controller : generally used with differential drive robots
• TEB Controller : generally used with car-like robots

3. Launch of the navigation coordinator

The node that calculates path planning and the node that creates wheel commands to follow that path have both been seen before. The node that coordinates the node that makes the request to the path planner node for a path and then makes the request to the controller to move the robot along it comes next. Bt navigator is the name of that node.

4. Launch of the recoveries_server

What happens if the robot is unable to locate a reliable route to the specified goal? What if the robot becomes impassed in the midst and is unable to proceed? The robot uses recovery behaviors in these scenarios. Behavior trees are used to call these straightforward, predefined actions, which typically resolve the issue. The node in charge of carrying out the recovery behaviors is recoveries server. For instance, when it thinks the robot is stuck, the bt navigator will contact the recoveries server.

Currently, there are three recovery behaviors available in Nav2:

• spin - performs an in-place rotation by a given angle
• backup - performs a linear translation by a given distance
• wait - brings the robot to a stationary state

In the config file, you should indicate which ones of those behaviors you want to load, so they will be ready to be called by the bt_navigator.

a) Create a new package in ros2_ws workspace named path_planner_server . b) Create launch & config directory at ros2_ws/src/path_planner_server c) Write a launch file to launch the path planner with the name pathplanner.launch.py where all the path planner nodes are properly launched d) Create a YAML file for each node that requires it in the config directory. You can use the config files shown above. e) Compile this new path planner package. Remember to modify the setup.py appropriately. Remember to source the new install. f) Launch the localization system from the previous lesson. g) Open RVIZ:

• Also, load the localization_rviz_config file. Then add the Path display and configure it for the /plan topic
• configure the Path display with Line Style: Billboards and then provide a Line Width of 0.05
• Save this RVIZ configuration inside the ros2_ws/src directory with the name pathplanning_rviz_config .

h) Use RVIZ to provide the initial robot location. i) Launch the path planner with the new launch file created.