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 allign laser scans and estimate
the change of pose.

COMPILATION

- pk-config should have the player package pointing to the libs and cflags
- 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

USE

The file "amigo_mricp.cfg" provides an example of driver use for an 
amigo (or pioneer) robot with a mounted Hokuyo URG laser.

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 postirior (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,
    (make sure the directory exists)
- 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_Driver"
  requires ["position:0" "laser:0"]
  provides ["position:1" "opaque:0" "map:0"]
  plugin "MRICP.so"
  MINR 0.05
  MAXR 3.9
  period 1
  map_resolution 0.6
  map_path maps/
  number_of_lasers 2
  start_in 1
  interpolate 0
  robot_id 1
  NIT 15
  log 1
)
|****************|
| STAGE TESTING  |
|****************|

- 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"
		  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
