MRICP

From CAS-Robotics

Jump to: navigation, search

Contents

MRICP driver for player/stage

This is a Map Reference ICP plugin Driver for real time Map building and Localization using Map Reference Iterative Closest Point to align laser scans and estimate the change of pose. This driver is created to support UTS CAS mobile Platforms, it can be quite useful in a lot of applications where easy Map building and localization are needed. I would appreciate any feedback, recommendations and bug reporting that can lead to improving the performance of this driver.

Download

Compilation and Usage

To compile the driver successfully, you should:

  • Have pkg-config pointing correctly to the player/stage.
  • Use the make file script to compile the driver and generate the shared object "MrIcpDriver.so" 
make

To install:

  • Copy MrIcpDriver.so to /usr/local/lib/ or
  • Create a symbolic link where you're running your configuration file 
ln -s MRICP_PATH/MrIcpDriver.so

where MRICP_PATH is the path where the mricp driver is compiled

Specifications / Operation

  • Upon start, the driver initializes the initial pose to [0 0 0] m,m,rd
  • When the driver receives a PLAYER_POSITION_RESET_ODOM_REQ configuration request, it: 
 1- Saves the current map patch and starts a new one.
 2- Raves the pose of the new patch in respect to the old one.
 3- It resets the Robot pose to[0 0 0].
 4- Resets the Occupancy grid probability and assumes all the cells unknown.
  • The driver saves the information about the map patches and their position relative to the previous patch map_path/map_configs.txt where map_path is the path of the directory to contain the maps specified in the driver configuration file.
  • The driver computes the Occupancy Grid using Bayesian probability update. It computes the uncertainty in the sensor readings and uses the prior knowledge of each cell to get the posterior (new estimation) of the cells state.

Configuration Parameters

- MAXR (double)
  - Default: "7.8"
  - Maximium Laser Range 
- MINR (double)
  - Default: "0.05"
  - Minimium Laser Range
- period (double)
  - Default: "0.2"
  - Time in sec between scans to be matched. The smaller the value is the more accurate maps you will get on the expense of CPU usage.
- map_resolution (double)
  - Default: "0.05"
  - Pixel resolution in meter of the map to be build 
- map_size (double)
  - Default: 20
  - This is defined from the origin to the boundary, so is actually half the size of the map PATCH, not the whole map.
- interpolate (bool)
  - Default "1"
  - 0 - Simple ICP, 1 - ICP with interpolation
- NIT (int)
  - Default "10"
  - Number of iterations for each scan-matching.
- gate1 (float)
  - Default "0.5"
  - 1st data association gate for each point in scan
- gate2 (float)
  - Default "0.05"
  - 2nd data association gate for each point in scan
- debug (bool)
  - Defult: 0 
  - Display Debug Messeges
- Log (bool)
  - Default: 0
  - Loggs the Odom Data (x,y,theta,ltics,rtics,lspeed,rspeed)
- map_path(string)
  - Default: "maps/"
  - Specifies the locations where patches and logs are to be saved
- start_in(int)
  - Default : 2
  - Delay Before starting, unit is in seconds
- robot_id(int)
  - Default : 0
  - The Robot id assigned for map identification
- number_of_laser(int)
  - Default : 1
  - The number of lasers to be used in the scan matching (index starts from 0) all lasers should be declared in the requires section
- playerv_debug (bool)
  - Default : 0
  - If enabled, the map occupancy will be represented by +1, 0, -1 (occupied, unoccupied, unknown), other wise, the probability will be scaled from
    0 - 255
- laserX_ranges tuple of (int) where X is an int 
  - Default : [-90 90]
  - Determines the acceptable laser scan ranges, even number of elements should exist in the tuple, smaller range should be first followed by the larger
    range. You will have to manually assign the value of X: eg. in ur configuration file u should have something like this for 2 lasers:
        number_of_lasers 2
        laser0_ranges [-90 -70 -50 -30 -10 90]
        laser1_ranges [-120 120]
    - this represent the following acceptable ranges:
      for Laser:0 [-90 -70] [-50 -30] [-10 90]
      for laser:1 [-120 120]
- use_max_range (float)
  - Default: 0
  - Specify if you want to use ranges more than the max to update empty spaces in Occupancy grid map, if it's not zero , then the range specified will
    be used to update the cells within that range only (usefull to reduce the effect of false returns)
- sparse_scans_rate (int)
  - Default: 1
  - Specifies the number of laser scan samples resolution, 1 means all beams, 2 means every take one every 2 and so on. (OG will not be affected by
    this, all samples will be use for OG) it reduces the CPU usage.
- use_odom (bool)
  - Default: 0
  - Specify if you want to use the underlying poisition driver's odom in laser scan correction the existance of an underlying position driver no longer 
    means that u are using the odom automatically since it can be used for passing velocity commands.
- free_space_prob (float) between 0 and 1
  - Default: 0.4
  - The probability to be used for updating the free space , lower values will help in reducing the false readings effect.
- map_saving_period (float)
  - Default : 10 sec
  - Determines how long time we should wait before saving the map.

Examples

This is an example on how to use the configuration parameters in your configurations file. 

driver
(
  name "MrIcpDriver"
  provides ["position2d:1" "map:0"]
  requires ["position2d:0" "laser:0"]
  number_of_lasers 1
  laser0_ranges [-120 120]
  playerv_debug 0
  period 0.2
  MAXR 3.9
  MINR 0.05
  map_resolution 0.05
  map_saving_period 5
  map_size 10
  use_max_range 1
  sparse_scans_rate 3
  map_path "logs/"
  debug 0
  alwayson 1
  log 1
)

Testing the Driver

  • The directory stage_test contains a stage configuration for testing mricp. Simply run: 
player stage_mricp.cfg
  • Also in the stage_test directory is a simple client program, which controls a robot with arrow keys and sends PLAYER_POSITION_RESET_ODOM_REQ configuration requests when the "c" key is pressed. Compile this utility with the "./compile_tclient" command.
  • To test multiple laser support, modify the world file and add X laser models to your robot (X is the number of lasers). Then modify your configuration file so that stage provides that X laser interfaces.
  • Note: Laser pose is essential for the Mricp to work, specify the correct laser poses.
  • Example : your .world file should contain something like this for 2 Laser support 
define sicklaser1 laser
(
  range_min 0.0
  range_max 8.0
  fov 180.0
  samples 180
  color "LightBlue"
  size [ 0.14 0.14 ]
  pose [0.8 -0.15 0 ]
)
define sicklaser2 laser
(
  range_min 0.0
  range_max 8.0
  fov 180.0
  samples 180
  color "LightBlue"
  size [ 0.14 0.14 ]
  pose [-0.10 0.15 -180 ]
)
# extend the wheelchair definition from wheelchair.inc
wheelchair
( 
   name "robot"
   pose [14.2 -4.6 180]
   sicklaser1()
   sicklaser2()
)

and your configuration should contain something like: 

driver
(
  name "stage"
  plugin "libstageplugin"
  provides ["simulation:0"]
  worldfile "rescue.world"
)
driver
( 
  name "stage"
  provides ["position2d:0" "laser:0"]
  model "robot" 
)

driver
(
  #name "mricp" if you are using player version>= 2.1
  name "MrIcpDriver" # if you are using the plugin
  provides ["position2d:1" "map:0"]
  requires ["position2d:0" "laser:0"]
  gate1 0.5
  gate2 0.05
  use_odom 1
  number_of_lasers 1
  laser0_ranges [-120 120]
  playerv_debug 0
  period 0.2
  MAXR 3.9
  MINR 0.05
  map_resolution 0.05
  map_saving_period 5
  map_size 16
  use_max_range 0
  sparse_scans_rate 1
  map_path "logs/"
  debug 0
  alwayson 1
  log 1
)

Image:map.png

Retrieved from "http://www.tarektaha.com/wiki/index.php/MRICP"
Personal tools