#!/usr/bin/perl

sub fix_heading {
    local($input_angle) = shift @_;
    my($pi) = 3.14159;
    local($angle) = $input_angle - $pi/2;

    while ($angle > $pi)
    {
	$angle -= $pi*2;
    }
    while ($angle < -$pi)
    {
	$angle += $pi*2;
    }

    return $angle;
}

$range_sf = 1.0;
$odo_sf = 1.0;
$laser_delay = 0.1;

$linenum = 0;
$num_odo = 0;
$num_sonar = 0;
$num_laser = 0;

$geom_msg = << 'eot';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOG FILE:       carmen.alog
%% FILE OPENED ON  Tue Dec 31 14:46:01 2002
%% LOGSTART   0.0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0.010 SENSOR_GEOMETRY iB21Sonar sonar23={x=0.0348113,y=0.264418,thetaDeg=82.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar23={x=0.0348113,y=0.264418,thetaDeg=82.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar22={x=0.102062,y=0.246399,thetaDeg=67.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar21={x=0.162357,y=0.211587,thetaDeg=52.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar20={x=0.211587,y=0.162357,thetaDeg=37.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar19={x=0.246399,y=0.102062,thetaDeg=22.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar18={x=0.264418,y=0.0348113,thetaDeg=7.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar17={x=0.264418,y=-0.0348113,thetaDeg=-7.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar16={x=0.246399,y=-0.102062,thetaDeg=-22.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar15={x=0.211587,y=-0.162357,thetaDeg=-37.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar14={x=0.162357,y=-0.211587,thetaDeg=-52.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar13={x=0.102062,y=-0.246399,thetaDeg=-67.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar12={x=0.0348113,y=-0.264418,thetaDeg=-82.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar11={x=-0.0348113,y=-0.264418,thetaDeg=-97.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar10={x=-0.102062,y=-0.246399,thetaDeg=-112.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar9={x=-0.162357,y=-0.211587,thetaDeg=-127.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar8={x=-0.211587,y=-0.162357,thetaDeg=-142.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar7={x=-0.246399,y=-0.102062,thetaDeg=-157.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar6={x=-0.264418,y=-0.0348113,thetaDeg=-172.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar5={x=-0.264418,y=0.0348113,thetaDeg=172.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar4={x=-0.246399,y=0.102062,thetaDeg=157.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar3={x=-0.211587,y=0.162357,thetaDeg=142.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar2={x=-0.162357,y=0.211587,thetaDeg=127.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar1={x=-0.102062,y=0.246399,thetaDeg=112.5},
0.010 SENSOR_GEOMETRY iB21Sonar sonar0={x=-0.0348113,y=0.264418,thetaDeg=97.5},
0.010 SENSOR_GEOMETRY iB21Laser laser179={x=0,y=0,thetaDeg=179.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser178={x=0,y=0,thetaDeg=178.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser177={x=0,y=0,thetaDeg=177.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser176={x=0,y=0,thetaDeg=176.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser175={x=0,y=0,thetaDeg=175.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser174={x=0,y=0,thetaDeg=174.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser173={x=0,y=0,thetaDeg=173.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser172={x=0,y=0,thetaDeg=172.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser171={x=0,y=0,thetaDeg=171.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser170={x=0,y=0,thetaDeg=170.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser169={x=0,y=0,thetaDeg=169.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser168={x=0,y=0,thetaDeg=168.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser167={x=0,y=0,thetaDeg=167.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser166={x=0,y=0,thetaDeg=166.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser165={x=0,y=0,thetaDeg=165.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser164={x=0,y=0,thetaDeg=164.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser163={x=0,y=0,thetaDeg=163.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser162={x=0,y=0,thetaDeg=162.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser161={x=0,y=0,thetaDeg=161.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser160={x=0,y=0,thetaDeg=160.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser159={x=0,y=0,thetaDeg=159.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser158={x=0,y=0,thetaDeg=158.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser157={x=0,y=0,thetaDeg=157.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser156={x=0,y=0,thetaDeg=156.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser155={x=0,y=0,thetaDeg=155.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser154={x=0,y=0,thetaDeg=154.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser153={x=0,y=0,thetaDeg=153.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser152={x=0,y=0,thetaDeg=152.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser151={x=0,y=0,thetaDeg=151.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser150={x=0,y=0,thetaDeg=150.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser149={x=0,y=0,thetaDeg=149.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser148={x=0,y=0,thetaDeg=148.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser147={x=0,y=0,thetaDeg=147.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser146={x=0,y=0,thetaDeg=146.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser145={x=0,y=0,thetaDeg=145.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser144={x=0,y=0,thetaDeg=144.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser143={x=0,y=0,thetaDeg=143.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser142={x=0,y=0,thetaDeg=142.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser141={x=0,y=0,thetaDeg=141.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser140={x=0,y=0,thetaDeg=140.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser139={x=0,y=0,thetaDeg=139.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser138={x=0,y=0,thetaDeg=138.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser137={x=0,y=0,thetaDeg=137.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser136={x=0,y=0,thetaDeg=136.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser135={x=0,y=0,thetaDeg=135.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser134={x=0,y=0,thetaDeg=134.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser133={x=0,y=0,thetaDeg=133.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser132={x=0,y=0,thetaDeg=132.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser131={x=0,y=0,thetaDeg=131.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser130={x=0,y=0,thetaDeg=130.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser129={x=0,y=0,thetaDeg=129.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser128={x=0,y=0,thetaDeg=128.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser127={x=0,y=0,thetaDeg=127.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser126={x=0,y=0,thetaDeg=126.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser125={x=0,y=0,thetaDeg=125.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser124={x=0,y=0,thetaDeg=124.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser123={x=0,y=0,thetaDeg=123.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser122={x=0,y=0,thetaDeg=122.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser121={x=0,y=0,thetaDeg=121.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser120={x=0,y=0,thetaDeg=120.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser119={x=0,y=0,thetaDeg=119.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser118={x=0,y=0,thetaDeg=118.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser117={x=0,y=0,thetaDeg=117.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser116={x=0,y=0,thetaDeg=116.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser115={x=0,y=0,thetaDeg=115.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser114={x=0,y=0,thetaDeg=114.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser113={x=0,y=0,thetaDeg=113.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser112={x=0,y=0,thetaDeg=112.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser111={x=0,y=0,thetaDeg=111.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser110={x=0,y=0,thetaDeg=110.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser109={x=0,y=0,thetaDeg=109.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser108={x=0,y=0,thetaDeg=108.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser107={x=0,y=0,thetaDeg=107.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser106={x=0,y=0,thetaDeg=106.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser105={x=0,y=0,thetaDeg=105.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser104={x=0,y=0,thetaDeg=104.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser103={x=0,y=0,thetaDeg=103.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser102={x=0,y=0,thetaDeg=102.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser101={x=0,y=0,thetaDeg=101.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser100={x=0,y=0,thetaDeg=100.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser99={x=0,y=0,thetaDeg=99.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser98={x=0,y=0,thetaDeg=98.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser97={x=0,y=0,thetaDeg=97.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser96={x=0,y=0,thetaDeg=96.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser95={x=0,y=0,thetaDeg=95.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser94={x=0,y=0,thetaDeg=94.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser93={x=0,y=0,thetaDeg=93.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser92={x=0,y=0,thetaDeg=92.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser91={x=0,y=0,thetaDeg=91.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser90={x=0,y=0,thetaDeg=90.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser89={x=0,y=0,thetaDeg=89.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser88={x=0,y=0,thetaDeg=88.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser87={x=0,y=0,thetaDeg=87.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser86={x=0,y=0,thetaDeg=86.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser85={x=0,y=0,thetaDeg=85.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser84={x=0,y=0,thetaDeg=84.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser83={x=0,y=0,thetaDeg=83.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser82={x=0,y=0,thetaDeg=82.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser81={x=0,y=0,thetaDeg=81.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser80={x=0,y=0,thetaDeg=80.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser79={x=0,y=0,thetaDeg=79.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser78={x=0,y=0,thetaDeg=78.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser77={x=0,y=0,thetaDeg=77.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser76={x=0,y=0,thetaDeg=76.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser75={x=0,y=0,thetaDeg=75.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser74={x=0,y=0,thetaDeg=74.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser73={x=0,y=0,thetaDeg=73.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser72={x=0,y=0,thetaDeg=72.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser71={x=0,y=0,thetaDeg=71.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser70={x=0,y=0,thetaDeg=70.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser69={x=0,y=0,thetaDeg=69.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser68={x=0,y=0,thetaDeg=68.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser67={x=0,y=0,thetaDeg=67.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser66={x=0,y=0,thetaDeg=66.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser65={x=0,y=0,thetaDeg=65.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser64={x=0,y=0,thetaDeg=64.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser63={x=0,y=0,thetaDeg=63.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser62={x=0,y=0,thetaDeg=62.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser61={x=0,y=0,thetaDeg=61.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser60={x=0,y=0,thetaDeg=60.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser59={x=0,y=0,thetaDeg=59.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser58={x=0,y=0,thetaDeg=58.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser57={x=0,y=0,thetaDeg=57.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser56={x=0,y=0,thetaDeg=56.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser55={x=0,y=0,thetaDeg=55.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser54={x=0,y=0,thetaDeg=54.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser53={x=0,y=0,thetaDeg=53.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser52={x=0,y=0,thetaDeg=52.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser51={x=0,y=0,thetaDeg=51.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser50={x=0,y=0,thetaDeg=50.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser49={x=0,y=0,thetaDeg=49.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser48={x=0,y=0,thetaDeg=48.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser47={x=0,y=0,thetaDeg=47.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser46={x=0,y=0,thetaDeg=46.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser45={x=0,y=0,thetaDeg=45.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser44={x=0,y=0,thetaDeg=44.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser43={x=0,y=0,thetaDeg=43.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser42={x=0,y=0,thetaDeg=42.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser41={x=0,y=0,thetaDeg=41.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser40={x=0,y=0,thetaDeg=40.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser39={x=0,y=0,thetaDeg=39.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser38={x=0,y=0,thetaDeg=38.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser37={x=0,y=0,thetaDeg=37.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser36={x=0,y=0,thetaDeg=36.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser35={x=0,y=0,thetaDeg=35.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser34={x=0,y=0,thetaDeg=34.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser33={x=0,y=0,thetaDeg=33.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser32={x=0,y=0,thetaDeg=32.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser31={x=0,y=0,thetaDeg=31.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser30={x=0,y=0,thetaDeg=30.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser29={x=0,y=0,thetaDeg=29.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser28={x=0,y=0,thetaDeg=28.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser27={x=0,y=0,thetaDeg=27.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser26={x=0,y=0,thetaDeg=26.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser25={x=0,y=0,thetaDeg=25.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser24={x=0,y=0,thetaDeg=24.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser23={x=0,y=0,thetaDeg=23.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser22={x=0,y=0,thetaDeg=22.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser21={x=0,y=0,thetaDeg=21.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser20={x=0,y=0,thetaDeg=20.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser19={x=0,y=0,thetaDeg=19.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser18={x=0,y=0,thetaDeg=18.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser17={x=0,y=0,thetaDeg=17.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser16={x=0,y=0,thetaDeg=16.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser15={x=0,y=0,thetaDeg=15.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser14={x=0,y=0,thetaDeg=14.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser13={x=0,y=0,thetaDeg=13.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser12={x=0,y=0,thetaDeg=12.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser11={x=0,y=0,thetaDeg=11.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser10={x=0,y=0,thetaDeg=10.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser9={x=0,y=0,thetaDeg=9.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser8={x=0,y=0,thetaDeg=8.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser7={x=0,y=0,thetaDeg=7.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser6={x=0,y=0,thetaDeg=6.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser5={x=0,y=0,thetaDeg=5.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser4={x=0,y=0,thetaDeg=4.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser3={x=0,y=0,thetaDeg=3.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser2={x=0,y=0,thetaDeg=2.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser1={x=0,y=0,thetaDeg=1.5}, 
0.010 SENSOR_GEOMETRY iB21Laser laser0={x=0,y=0,thetaDeg=0.5}, 
eot

print $geom_msg;

$pi = 3.14159;

$log_start_time = 0;

# CARMEN Logfile
# file format is one message per line
# message_name [message contents] ipc_timestamp ipc_hostname logger_timestamp
# message formats defined: PARAM SYNC ODOM FLASER RLASER
# PARAM param_name param_value
# SYNC tagname
# ODOM x y theta tv rv accel
# FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta
# RLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta

$odo_vx = 0.0;
$odo_vy = 0.0;
$odo_vh = 0.0;

while (<>)
{

    if (/^ODOM/)
    {
	@msg = split;
	$odo_x     = $msg[1] * $odo_sf;
	$odo_y     = $msg[2] * $odo_sf;
	$odo_theta = $msg[3];

	$odo_vx    = $msg[4] * $odo_sf;
	$odo_vh    = $msg[5];
	$odo_accel = $msg[6];

	$ipc_timestamp = $msg[7];
	$ipc_name      = $msg[8];
	$log_timestamp = $msg[9];

	if ($log_start_time == 0)
	{
	    $log_start_time = $log_timestamp;
	}
	$time = $log_timestamp;

	$speed = sqrt($odo_vx^2 + $odo_vy^2);

	$odo_theta = &fix_heading($odo_theta);

	#print "$time B21_ODOMETRY $ipc_name x=$odo_x,y=$odo_y,h=$odo_theta,vx=$odo_vx,vy=$odo_vy,vh=$odo_vh,speed=$speed,time=$ipc_timestamp\n";

	$num_odo++;
    }

    if (/^FLASER/)
    {
	@msg = split;
	$num_readings = $msg[1];
	@ranges = @msg[2..($num_readings+1)];
	$x     = $msg[$num_readings+2] * $odo_sf;
	$y     = $msg[$num_readings+3] * $odo_sf;
	$theta = $msg[$num_readings+4];
	$odo_x     = $msg[$num_readings+5] * $odo_sf;
	$odo_y     = $msg[$num_readings+6] * $odo_sf;
	$odo_theta = $msg[$num_readings+7];

	$theta = &fix_heading($theta);
	$odo_theta = &fix_heading($odo_theta);

	$ipc_timestamp = $msg[$num_readings+8];
	$ipc_name      = $msg[$num_readings+9];
	$log_timestamp = $msg[$num_readings+10];

	$laser_time = $ipc_timestamp - $laser_delay;

	if ($log_start_time == 0)
	{
	    $log_start_time = $log_timestamp;
	}
	$time = $log_timestamp;
	$odotime = $time - 0.001;

	$speed = sqrt($odo_vx^2 + $odo_vy^2);

	print "$odotime B21_ODOMETRY $ipc_name x=$odo_x,y=$odo_y,h=$odo_theta,vx=$odo_vx,vy=$odo_vy,vh=$odo_vh,speed=$speed,time=$ipc_timestamp\n";

	for $i (0..$#ranges)
	{
	    $ranges[$i] = $ranges[$i] * $range_sf;
	}
	$data_string = join(", ",@ranges);

	print "$time B21_LASER_RANGE $ipc_name time=$laser_time,data=[$num_readings]{$data_string}\n";
	$num_laser++;
    }

    $linenum++;
    if ($linenum % 1000 == 0)
    {
	print STDERR "line $linenum odo:$num_odo sonar:$num_sonar laser:$num_laser $dtime\n";
    }
}

print STDERR "line $linenum odo:$num_odo sonar:$num_sonar laser:$num_laser\n";
print STDERR "Done.\n"

