This is the MRICP driver for player/stage.

This Driver is used as a localizer and Occupancy Grid Map Builder using simple Bayesian
Probability Update. It uses Iterative Closest Point method to align laser scans and estimate
the change of pose.

USE

The driver requires at least one laser interface, and optionally a position
interface (the latter is required if you actually want to drive the
robot via this driver).  The driver provides a position interface which
returns the position of the robot in the current patch as calculated by
mricp.

	Specifications / Operation
- Upon start, the driver initializes the initial pose to [0 0 0] m,m,rd
- When the driver recieves 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.

|********************|
| CONFIG PARAMETERS  |
|********************|

- MAXR (double)
  - Default: "7.8"
  - Maximium Laser Range, This must be smaller than the laser's maximum range,
    especially for SICK lasers.  Use 3.9 for Hokuyo URG.
- MINR (double)
  - Default: "0.05"
  - Minimium Laser Range
- period (double)
  - Default: "0.1"
  - Time in sec defining how frequently to grab laser scans
    for scan matching. It is not a good idea to use maximum scans available on a SICK.
    0.1 to 0.2 works well depending on robot turnrate and speed.
- map_resolution (double)
  - Default: "0.05"
  - Pixel resolution in meter of the map to be build , a number close to the laser
    precision is wise.
- 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
@par Example

@verbatim
driver
(
  name "mricp"
  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 4
  sparse_scans_rate 3
  map_path "logs/"
  debug 0
  alwayson 1
  log 1
)
|****************|
| STAGE TESTING  |
|****************|

- The directory stage_test contains a stage configuration for testing
  mricp.  Simply run:
  "player stage_mricp.cfg"

- 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"
		  provides ["position:0" "laser:0" "laser:1"]
		  model "robot"
		)
		driver
		(
		  name "MrIcpDriver"
		  plugin "MrIcpDriver.so"
		  provides ["position:1" "map:0"]
		  requires ["laser:0" "laser:1" "position:0"]
		  map_size 10
		  map_resolution 0.05
		  number_of_lasers 2
		  period 0.1
		  MAXR 7.5
		  MINR 0.05
		  debug 0
		  alwayson 1
		  log 1
		)

JP 21/4/06
modified
TT 16/5/06
