ToshikiNakamura0412/emcl_ros

ROS implementation of emcl (mcl with expansion resetting)

emcl_ros

ROS implementation of emcl (mcl with expansion resetting)

Support Dynamic Reconfigure

Environment

• Ubuntu 20.04
• ROS Noetic

Install and Build

# clone repository
cd /path/to/your/catkin_ws/src
git clone https://github.com/ToshikiNakamura0412/emcl_ros.git

# build
cd /path/to/your/catkin_ws
rosdep install -riy --from-paths src --rosdistro noetic # Install dependencies
catkin build emcl_ros -DCMAKE_BUILD_TYPE=Release        # Release build is recommended

How to use

roslaunch emcl_ros emcl.launch

Running the demo

# clone repository
cd /path/to/your/catkin_ws/src
git clone https://github.com/ToshikiNakamura0412/scan_to_pcl_ros.git
git clone https://github.com/ToshikiNakamura0412/gyrodometry_ros.git
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git

# build
cd /path/to/your/catkin_ws
rosdep install -riy --from-paths src --rosdistro noetic
catkin build -DCMAKE_BUILD_TYPE=Release

# run demo
## terminal 1
export TURTLEBOT3_MODEL=burger
roslaunch emcl_ros test.launch
## terminal 2
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

Use gyrodometry

roslaunch emcl_ros test.launch use_gyrodom:=true

Node I/O

Nodes

emcl

Published Topics

• /emcl_pose (geometry_msgs/PoseWithCovarianceStamped)
  • The estimated pose of the robot
• /tf (tf2_msgs/TFMessage)
  • tf (from the global frame to the odom frame)
• ~<name>/particle_cloud (geometry_msgs/PoseArray)
  • The particle cloud of mcl

Subscribed Topics

• /cloud (sensor_msgs/PointCloud2)
  • The input pointcloud data
  • If ~<name>/use_cloud is false, this topic is not used
• /initialpose (geometry_msgs/PoseWithCovarianceStamped)
  • The initial pose of the robot
• /odom (nav_msgs/Odometry)
  • The odometry data
• /scan (sensor_msgs/LaserScan)
  • The input laser scan data (default)

Parameters

EMCL Parameters

• ~<name>/expansion_position_dev (float, default: 0.07 [m]):
  The standard deviation of the expansion noise in position
• ~<name>/expansion_orientation_dev (float, default: 0.2 [rad]):
  The standard deviation of the expansion noise in orientation
• ~<name>/init_x (float, default: 0.0 [m]):
  The initial x position of the robot
• ~<name>/init_y (float, default: 0.0 [m]):
  The initial y position of the robot
• ~<name>/init_yaw (float, default: 0.0 [rad]):
  The initial yaw of the robot
• ~<name>/init_position_dev (float, default: 0.1 [m]):
  The standard deviation of the initial noise in position
• ~<name>/init_orientation_dev (float, default: 0.05 [rad]):
  The standard deviation of the initial noise in orientation
• ~<name>/laser_step (int, default: 4):
  The step of the laser scan
• ~<name>/likelihood_th (float, default: 0.002):
  The threshold of the likelihood
• ~<name>/particle_num (int, default: 420):
  The number of particles
• ~<name>/reset_count_limit (int, default: 3):
  The limit of the reset count
• ~<name>/sensor_noise_ratio (float, default: 0.03):
  The ratio of sensor noise to the actual sensor noise
• ~<name>/use_cloud (bool, default: False):
  If true, use pointcloud instead of laser scan

OdomModel Parameters

• ~<name>/ff (float, default: 0.17 [m]):
  Standard deviation of forward noise per forward
• ~<name>/fr (float, default: 0.0005 [m]):
  Standard deviation of forward noise per rotation
• ~<name>/rf (float, default: 0.13 [rad]):
  Standard deviation of rotation noise per forward
• ~<name>/rr (float, default: 0.2 [rad]):
  Standard deviation of rotation noise per rotation

Scan Parameters

If pointcloud is used, following parameters are used.

• ~<name>/range_min (float, default: 0.12 [m]):
  The minimum range of the sensor
• ~<name>/range_max (float, default: 3.5 [m]):
  The maximum range of the sensor

Common Parameters

• ~<name>/use_dynamic_reconfigure (bool, default: False):
  If true, use dynamic reconfigure

References

• 上田隆一, "詳解 確率ロボティクス Pythonによる基礎アルゴリズムの実装", Kodansya, 2019
• https://github.com/ryuichiueda/emcl
• 赤井直紀, "LiDARを用いた高度自己位置推定システム - 移動ロボットのための自己位置推定の高性能化とその実装例", コロナ社, 2022
• https://github.com/NaokiAkai/ALSEdu