📍 Localization

Section

🧭 Navigation - high-level documentation

Localization refers to the process of determining the position and orientation of the robot with respect to a fixed reference frame in a given environment.

PAL Robots Localization pipeline is compliant with the REP-105 ROS Standard which defines three reference frames for localization.

According to this standard, the purpose of the localization pipeline is to calculate the transform between the following frames:

• base_link (or base_footprint) : Reference frame rigidly attached to the mobile robot base. This link can be attached to the base in any arbitrary position or orientation.

• odom : Reference frame that is used to represent the continuous motion of the robot. The pose of the robot in the odom frame can drift over time, without any bounds, making the odom frame useless as a long-term global reference. However, the pose of a robot in the odom frame is guaranteed to be continuous, meaning that the pose of a mobile platform in the odom frame always evolves smoothly, without discrete jumps. In a typical setup, the odom frame is computed based on an odometry source, such as wheel odometry, visual odometry or an inertial measurement unit.

• map : World-fixed reference frame representing an initial, reference pose selected on a map. The pose of a mobile platform, relative to the map frame, should not significantly drift over time. The map frame is not continuous, meaning the pose of a mobile platform in the map frame can change in discrete jumps at any time. In a typical setup, a localization component constantly re-computes the robot poses in the map frame based on sensor observations, therefore eliminating drift, but causing discrete jumps when new sensor information arrives.

The localization pipeline can be further divided into two main components, the Local Localization and the Global Localization.

Local Localization

The Local Localization pipeline is responsible for estimating the transform between the base_footprint (or base_link) and the odom frames. It does so by calculating the robot’s odometry using the sensors available on the robot. As stated in the REP-105, this transform is guaranteed to be continuous, meaning that the pose of the robot in the odom frame always evolves continuously, without discrete jumps.

PAL Robots use an odometry system based on the fusion of wheel odometry, using wheel encoders, and a Laser Sensor, using a 2D LiDAR sensor.

Warning

The dlo_ros and the /dlo_ros/odom are only available if your robot is equipped with a 2D Laser Sensor. Otherwise only the /mobile_base_controller/odom will be available.

Given this Local Localization pipeline, the /mobile_base_controller/odom contains the wheel odometry as estimated by the wheel encoders, and the /dlo_ros/odom contains the fused information of the laser odometry and the wheel odometry.

Global Localization

The Global Localization pipeline is responsible for estimating the transform between the map and the odom frames. It does so by using the Monte Carlo Localization (MCL) algorithm implemented in the Nav2 AMCL package.

The purpose of this package is to correct the odometry drift by using a 2D LiDAR and compare its scans with a map of the environment (OccupancyGrid).

Warning

The amcl is only available if your robot is equipped with a 2D Laser Sensor.

Usage

PAL Robots already come with some pre-configured and tested AMCL configurations that are customized for different environments and for different robots. To list all the available configurations, you can use the command:

ls /opt/pal/$PAL_DISTRO/share/pal_navigation_cfg_params/params/nav2_amcl/

By default, PAL Robots start in localization mode, meaning that the localization pipeline is automatically started when the robot was turned on.

Attention

Both the localization and the mapping pipelines will publish a transform between the frames map and base_footprint. For this reason, you should not start both pipelines at the same time. When starting the mapping pipeline make sure you first shut down the localization pipeline and vice-versa.

To stop the localization pipeline and all the nodes associated with it, you can use the command:

pal module stop localization

And to start it you can use the command:

pal module start localization

The localization will use the default map loaded by the map_server. If you want to use a different map, you can dynamically change it using the /map_server/load_map with the following command:

ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "map_url: '/home/pal/my_map.yaml'"

Otherwise, you can change the default map that the map_server loads, you can create a new configuration file. To do so, you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_map_server

And then, within this folder, you can create a new Map Server configuration file.

touch ~/.pal/pal_navigation_cfg_params/params/nav2_map_server/my_map_server.yaml

Then modify the newly created configuration file with the command:

vi ~/.pal/pal_navigation_cfg_params/params/nav2_map_server/my_map_server.yaml

In this configuration file, you can change the yaml_filename parameter to the path of the map you want to use by specifying the full path to the map.yaml file you want to use. Alternatively, you can use any of the available maps in the pal_maps package.

pal_navigation_cfg:
  ros__parameters:
    supported_robots:
    - some_pal_robot # e.g. tiago, omni_base, tiago_pro, etc.

__node_name__:
   ros__parameters:
      use_sim_time: ${use_sim_time}
      yaml_filename: "/your/path/to/map.yaml" # or /opt/pal/$PAL_DISTRO/share/pal_maps/maps/<SOME_MAP>/map.yaml

Once created, to start using your custom Map Server configuration, you need to set it in the navigation pipeline by modifying the <robot>_loc.yaml file in the <robot>_2dnav package.

sudo vi /opt/pal/$PAL_DISTRO/share/<robot>_2dnav/params/<robot>_loc.yaml

Attention

Make sure to replace the <robot> placeholder with the name of your robot (e.g. tiago, omni_base, tiago_pro, etc.).

Then, change the params field of the map_server to the name of your custom Map Server configuration file.

nodes:
   # Other nodes
   map_server:
     app: map_server
     params: [my_map_server]

Attention

Make sure that the name of the custom Map Server configuration file you use in the params field (my_map_server) is the same as the name of the file you created in the .pal folder (my_map_server.yaml).

Finally, to apply the changes and start using the new Map Server configuration, you need to restart the localization pipeline with the command:

pal module restart localization

Finally, the MCL algorithm requires an initial pose to start the localization process. This doesn’t need to be an accurate estimate of the initial pose of the robot, but can be an initial guess of an area on the map where the robot is located. This initial pose can be set either through the /initialpose topic with the command:

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
"{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, \
pose: {pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, \
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, \
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}}"

Note

Change the values of the position and orientation fields to the desired initial pose of the robot.

You can use the same /initialpose through the 2D Pose Estimate RViz Tool.

Alternatively, you can set an initial pose for the amcl using the /set_initial_pose service with the following command:

ros2 service call /set_initial_pose nav2_msgs/srv/SetInitialPose \
"pose: {header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, \
pose: {pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, \
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, \
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}}"

Configuration

In addition to the available PAL configurations for the amcl, you can create new configurations for your robot. To do so, you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_amcl

And then, within this folder, you can create a new AMCL configuration file.

touch ~/.pal/pal_navigation_cfg_params/params/nav2_amcl/my_amcl_config.yaml

Within the newly created my_amcl_config.yaml file, you can insert your custom AMCL parameters. For the list of available parameters, their meaning and how they affect the localization algorithm, you can refer to the Nav2 AMCL configuration guide. When creating a new Navigation configuration file, apart from the node parameters, you also need to specify the robot to which they refer.

pal_navigation_cfg:
   ros__parameters:
      supported_robots:
      - some_pal_robot # e.g. tiago, omni_base, tiago_pro, etc.

amcl:
   # Your AMCL Parameters

Once created, to start using your custom AMCL configuration, you need to set it in the localization pipeline by modifying the <robot>_loc.yaml file in the <robot>_2dnav package.

sudo vi /opt/pal/$PAL_DISTRO/share/<robot>_2dnav/params/<robot>_loc.yaml

And then, change the params field of the amcl to the name of your custom AMCL configuration file.

nodes:
   # Other nodes
   amcl:
      app: amcl
      params: [my_amcl_config]

Attention

Make sure that the name of the custom AMCL configuration file you use in the params field (my_amcl_config) is the same as the name of the file you created in the .pal folder (my_amcl_config.yaml).

Finally, to apply the changes and start using the new AMCL configuration, you need to restart the localization pipeline with the command:

pal module restart localization

ROS 2 API

To interact with the localization pipeline, you can refer to the following ROS 2 interfaces:

Topics	Services
/amcl_pose	/map_server/load_map
/dlo_ros/odom	/set_initial_pose
/initialpose
/mobile_base_controller/odom
/particle_cloud

References

• F. Dellaert, D. Fox, W. Burgard and S. Thrun, “Monte Carlo localization for mobile robots,” Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), Detroit, MI, USA, 1999, pp. 1322-1328 vol.2, doi: 10.1109/ROBOT.1999.772544.

• Thrun, Sebastian, Burgard, Wolfram and Fox, Dieter. Probabilistic Robotics. Cambridge, MA: MIT Press, 2005.