Emcl is an alternative Monte Carlo localization (MCL) package to amcl (http://wiki.ros.org/amcl). Differently from amcl, KLD-sampling and adaptive MCL are not implemented. Instead, the expansion resetting and other features are implemented12.
This package is ROS 2 version of ryuichiueda/emcl2.
- ROS 2 Humble Hawksbill
mkdir ros2_ws && cd ros2_ws
git clone git@github.com:CIT-Autonomous-Robot-Lab/emcl2_ros2.git ./src/emcl2_ros2
rosdep update
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
sudo apt install -y ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-turtlebot3-gazebo
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash
You may wait Gazebo to be initilalized. You can also use ./test/demo.bash, in which the following procedure is written.
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
ros2 launch nav2_bringup rviz_launch.py
ros2 launch emcl2 emcl2.launch.py params_file:=$(ros2 pkg prefix --share emcl2)/config/emcl2_quick_start.param.yaml map:=$(ros2 pkg prefix --share nav2_bringup)/maps/turtlebot3_world.yaml use_sim_time:=true
ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true
This node calculates the alpha value with a different algorithm than emcl_node in ryuichiueda/emcl. This node counts the particles that make lasers penetrate occupancy cells. Specifically, this node chooses some particles at a rate of extraction_rate and checks each of them with the following procedure:
- maps a set of laser scan on the occupancy grid map based on the pose of the particle
- judges the pose of the particle as wrong if all of lasers in a
range_threshold[rad] range penatrate occupancy grids
If the rate of the wrong particles is greater than alpha_threshold, the node invokes a reset.
This node also has a sensor resetting algorithm. When sensor_reset is true, a particle with laser penetration is dragged back from occupied cells.
| Name | Type | Description |
|---|---|---|
map |
nav_msgs/OccupancyGrid |
occupancy grid map |
scan |
sensor_msgs/LaserScan |
laser scans |
tf |
tf/tfMessage |
transforms |
initialpose |
geometry_msgs/PoseWithCovarianceStamped |
pose of particles for replacement |
| Name | Type | Description |
|---|---|---|
mcl_pose |
geometry_msgs/PoseWithCovarianceStamped |
the mean pose of the particles with covariance |
particlecloud |
geometry_msgs/PoseArray |
poses of the particles |
tf |
tf/tfMessage |
the transform from odom (which can be remapped via the odom_frame_id parameter) to map |
alpha |
std_msgs/Float32 |
marginal likelihood of particles after sensor update |
| Name | Type | Default | Description |
|---|---|---|---|
odom_freq |
int |
20 [Hz] | frequency of odometry update |
num_particles |
int |
500 | number of particles |
odom_frame_id |
string |
"odom" | the frame for odometry |
footprint_frame_id |
string |
"base_footprint" | the frame of the localized robot's base |
base_frame_id |
string |
"base_link" | the frame of the robot's base. It is used for calculating the position and orientation of the LiDAR |
global_frame_id |
string |
"map" | the frame for localization |
initial_pose_x |
double |
0.0 [m] | initial x coordinate of particles |
initial_pose_y |
double |
0.0 [m] | initial y coordinate of particles |
initial_pose_a |
double |
0.0 [rad] | initial yaw coordinate of particles |
odom_fw_dev_per_fw |
double |
0.19 [m/m] | standard deviation of forward motion noise by forward motion |
odom_fw_dev_per_rot |
double |
0.0001 [m/rad] | standard deviation of forward motion noise by rotational motion |
odom_rot_dev_per_fw |
double |
0.13 [rad/m] | standard deviation of rotational motion noise by forward motion |
odom_rot_dev_per_rot |
double |
0.2 [rad/rad] | standard deviation of rotational motion noise by rotational motion |
laser_likelihood_max_dist |
double |
0.2 meters | maximum distance to inflate occupied cells on the likelihood field map |
scan_increment |
int |
1 | increment number when beams are picked from their sequence |
alpha_threshold |
double |
0.5 | threshold of the alpha value for expansion resetting |
open_space_threshold |
double |
0.05 | threshold of the valid beam rate for expansion resetting |
expansion_radius_position |
double |
0.1 [m] | maximum change of the position on the xy-plane when the reset replaces a particle |
expansion_radius_orientation |
double |
0.2 [rad] | maximum change of the yaw angle when the reset replaces a particle |
extraction_rate |
double |
0.1 | rate of particles that are checked by the node |
range_threshold |
double |
0.3 [rad] | threshold of the range of lasers |
sensor_reset |
bool |
true | flag for sensor resettings |
The followings have never been implemented yet.
| Name | Type | Default | Description |
|---|---|---|---|
laser_min_range |
double |
0.0 [m] | threshold for discarding scans whose ranges are smaller than this value |
laser_max_range |
double |
100000000.0 [m] | threshold for discarding scans whose ranges are larger than this value |
- Implement service
- Fix a bug that caused particles to survive
Footnotes
-
R. Ueda: "Syokai Kakuritsu Robotics (lecture note on probabilistic robotics)," Kodansya, 2019. ↩
-
R. Ueda, T. Arai, K. Sakamoto, T. Kikuchi, S. Kamiya: Expansion resetting for recovery from fatal error in Monte Carlo localization - comparison with sensor resetting methods, IEEE/RSJ IROS, pp.2481-2486, 2004. ↩