Localization

The previous tutorial used the map you created to create global plans, but it allowed you to specify the start and goal position of the robot. However, that leads to the question, how do you know the robot's start position, aka how do you know where your robot is within the map? That question is answered using the localization process, and for the Stretch, that means the nav2_amcl package.

Particle Filters

Because the robot's sensors and odometry are inherently noisy, and the world may have changed since the map was created, the process of localization is done probabilistically using a particle filter. A particle filter creates an arbitrary number of candidate poses for where the robot could be (called particles). Each of these poses is evaluated using the robot's sensor data. The particles are also updated as the robot moves. The best scoring guess on any particular cycle is used as the robot's pose in the map.

The initial set of particles are randomized, and how the particles are updated is also random. This style of randomized algorithm is associated with Monte Carlo (a casino in Monaco), thus leading to the ROS packages name, nav2_amcl, where AMCL stands for Adaptive Monte Carlo Localization.

Initial Poses

The number of particles will be between the two parameters min_particles and max_particles.

By default, you have to give the algorithm an initial estimate of where the robot is. This can either be done using parameters by setting set_initial_pose to True, which will read the parameters initial_pose/x, initial_pose/y and initial_pose/yaw (all zero by default). Alternatively, in real world scenarios where you won't always have the same starting pose, you can draw the starting pose in RViz using the "2D Pose Estimate" tool, which will publish a message on the topic /initialpose which the algorithm will subscribe to.

Alternatively, you can start out with absolutely no information, and scatter the particles all over the map by calling the /reinitialize_global_localization service:

ros2 service call /reinitialize_global_localization std_srvs/srv/Empty {}

Sensor Scoring Algorithm

For each of the poses/particles, we must ask the question, how well does this fit with our sensor data?

Stretch 4 uses a model of the sensors called likelihood_field_prob (this is set using the laser_model_type param). This combines three different probabilities using weights for each of the laser readings that it uses.

• The probability of getting a "hit" (valid sensor reading) at the pose, based on the map and distance to obstacles. The weight on this is set by the z_hit parameter.
• The probability of getting a "random" reading. The weight on this is set by the z_rand parameter.
• The probability of failing to get a reading, and getting the maximum possible range instead. The weight on this depends on the other two parameters, since it should add to 1.

Parameter Tuning Tip: If it seems like the wrong particles are being selected as the best candidate, then tuning these weights is your best bet.

Motion Model

As the robot moves, the changes in position from the odometry are used to change the particles. If foward motion is reported from the odometry, then all of the particles' respective poses will be moved forward. Similarly, if odometry turns, the poses will turn.

However, there is some randomness / sampling present here as well. As good as the Stretch 4's odometry calculations are, there are hardly ever perfect readings in any robot. Hence, the poses are adjusted using a probabilistic motion model with multiple types of error. The error can stem from either translation/rotation and affect the estimated translation and rotation. The motion model errors are set using the following parameters.

• alpha1 - Noise in rotation from rotation
• alpha2 - Noise in rotation from translation
• alpha3 - Noise in translation from translation
• alpha4 - Noise in translation from rotation
• alpha5 - Additional translational noise for omnidirectional robots like Stretch 4.

The motion model itself is set by the robot_model_type, which for Stretch 4 is nav2_amcl::OmniMotionModel.

You can set update_min_d and update_min_a for the minimum distance and angle the robot needs to travel before the motion model is applied to each particle.

Experimenting with Localization

There is a launch file that you can use to experiment with the localization parameters with bagged data.

First, launch the demo with the map that you want to localize to:

ros2 launch stretch_nav2 localization_demo.launch.py map:=/path/to/map.yaml

Next, play back a bag file (possibly even the one you used to create the map), but only using some topics:

ros2 bag play /path/to/bagfile --topics /odom /robot_description /scan_filtered /tf_static --clock

(This way, we are not publishing the transforms that were created during the mapping process)

You can run these two commands and watch the localization process in rviz. To change the parameters, insert them into the amcl section of stretch_nav2/config/nav2_params_core.yaml.