diff --git a/.qrcode.swp b/.qrcode.swp
new file mode 100644
index 0000000000000000000000000000000000000000..6c79e61edf19552dcc2b4db852f246248727e735
Binary files /dev/null and b/.qrcode.swp differ
diff --git a/388box b/388box
new file mode 100644
index 0000000000000000000000000000000000000000..0faad864212037131d87165f6d2754ece30e5343
--- /dev/null
+++ b/388box
@@ -0,0 +1,4 @@
+0.0 0.0 1.8 0
+0.0 1.8 1.8 1.8
+0.0 0.0 0.0 1.8
+1.8 0.0 1.8 1.8
diff --git a/388line b/388line
new file mode 100644
index 0000000000000000000000000000000000000000..1371f2da9dbc329869a063330529943f74d17533
--- /dev/null
+++ b/388line
@@ -0,0 +1,9 @@
+ 0 1.0 2.3 1.0
+1.0 1.0 1.25 1.125
+1.25 1.125 1.5 1.375
+1.5 1.375 1.75 1.5
+1.75 1.5 4.25 1.5
+2.5 1.3 2.5 1.7
+3.0 1.3 3.0 1.7
+3.5 1.3 3.5 1.7
+4.0 1.3 4.0 1.7
diff --git a/388maze b/388maze
new file mode 100644
index 0000000000000000000000000000000000000000..e3b7156205ce05c593f046f74fa24df8032f6b66
--- /dev/null
+++ b/388maze
@@ -0,0 +1,13 @@
+0.0 0.0 3.75 0.0 kuk
+0.75 0.75 1.5 0.75 kuk1
+2.25 0.75 3.75 0.75
+0.0 1.5 0.75 1.5
+1.5 1.5 3.0 1.5
+0.75 2.25 2.25 2.25
+0.0 3.0 1.5 3.0
+0.0 0.0 0.0 3.75
+1.5 0.75 1.5 1.5
+2.25 2.25 2.25 3.75
+3.0 1.5 3.0 3.0
+3.75 0.0 3.75 3.75
+0.0 3.75 3.73 3.75
diff --git a/388project b/388project
new file mode 100644
index 0000000000000000000000000000000000000000..791c46270ca38ac5ea6bf19748e2d88ee079e18d
--- /dev/null
+++ b/388project
@@ -0,0 +1,18 @@
+0.0 0.0 1.8 0.0 bottom left
+2.2 0.0 4.0 0.0 bottom right
+0.0 5.0 1.8 5.0 top left
+2.2 5.0 4.0 5.0 top right
+0.0 0.0 0.0 1.8 left down
+0.0 3.2 0.0 5.0 left up
+4.0 0.0 4.0 1.8 right down
+4.0 3.2 4.0 5.0 right up
+0.9 3.1 1.7 3.1 maze bottom left
+2.3 3.1 3.1 3.1 maze bottom right
+1.7 2.5 1.7 3.1 maze left down
+0.9 3.1 0.9 4.3 maze left up
+2.3 2.5 2.3 3.1 maze right down
+3.1 3.1 3.1 4.3 maze right up
+1.5 3.7 2.5 3.7 maze middle horizontal
+2.0 3.7 2.0 4.3 maze middle vertical
+0.9 4.3 3.1 4.3 maze top
+
diff --git a/388wall b/388wall
new file mode 100644
index 0000000000000000000000000000000000000000..b5d333eb3dbf48605af181e6825adb15ac72746f
--- /dev/null
+++ b/388wall
@@ -0,0 +1 @@
+2.0 0.85 2.0 1.15
diff --git a/388world.world b/388world.world
new file mode 100644
index 0000000000000000000000000000000000000000..286b8bbab2d94d69f3bbe00d1c3fea98f0310630
--- /dev/null
+++ b/388world.world
@@ -0,0 +1,88 @@
+#Simulation environment for the 31388 course
+
+#Include SMR robot model together with basic map and URG laser scanner
+include "smr.inc"
+include "map.inc"
+include "urg-04lx.inc"
+include "guidemark.inc"
+
+interval_sim 10
+
+# configure the GUI window
+window
+(
+ size [ 600.000 750.000 ]
+ center [0 0.200]
+ scale 100.000
+ show_data 1 # 1=on 0=off
+)
+
+# load an environment bitmap
+floorplan
+(
+ bitmap "test.png"
+ size [5.000 6.500 0.300]
+ pose [0 0 0 0]
+ name "385world"
+)
+
+linemap
+(
+ bitmap "test.png"
+ size [5.000 6.500 0.010]
+ pose [0 0 0 0]
+ name "linemap"
+)
+
+
+
+# create a robot
+smr
+(
+ name "smr0"
+ pose [-1.500 -2.8 0 90]
+)
+
+guidemark
+(
+ name "gmk1"
+ pose [1.500 -2.000 0 180.000]
+
+ fiducial_return 1
+ fiducial( fov 60 )
+)
+
+
+guidemark
+(
+ name "gmk2"
+ pose [1.700 -2.000 0 180.000]
+
+ fiducial_return 2
+ fiducial( fov 60 )
+)
+
+guidemark
+(
+ name "gmk3"
+ pose [1.900 -2.000 0 180.000]
+
+ fiducial_return 3
+ fiducial( fov 60 )
+)
+
+
+guidemark
+(
+ name "gmk4"
+ pose [2.100 -2.000 0 180.000]
+
+ fiducial_return 4
+ fiducial( fov 60 )
+)
+
+box
+(
+ name "box1"
+ pose [-1 -2.5 0 0]
+)
diff --git a/4x4_big.png b/4x4_big.png
new file mode 100644
index 0000000000000000000000000000000000000000..084828ab0dd3e2a9a70b067dfe21926f1869084d
Binary files /dev/null and b/4x4_big.png differ
diff --git a/bash_src/client.sh b/bash_src/client.sh
new file mode 100644
index 0000000000000000000000000000000000000000..80338773b8e0eff363f625bffded4040d3794b6c
--- /dev/null
+++ b/bash_src/client.sh
@@ -0,0 +1,4 @@
+#!usr/bin/bash
+# run the client
+cd ..
+qclient -style a
diff --git a/bash_src/data.debug b/bash_src/data.debug
new file mode 100644
index 0000000000000000000000000000000000000000..3aab0882f7e1bdf7685ce2cff9211de57f8d7810
--- /dev/null
+++ b/bash_src/data.debug
@@ -0,0 +1,74 @@
+Debug file for the robot initialization
+---------------------------------------
+Thu May 21 16:11:28 2020
+---------------------------------------
+
+Module : robotinfo
+ type differential enum number 0
+ name smr
+
+Module : odometry
+ cl 0.000102
+ cr 0.000102
+ w 0.260000
+ robotlength 0.265000
+ ts 0.010000
+ maxtick 1000
+ control 0
+ enctype 0
+
+Module : motion control
+ ts 0.010000
+ line_gain 0.050000
+ line_tau 10.000000
+ line_alfa 0.300000
+ wall_gain 1.000000
+ wall_tau 0.700000
+ wall_alfa 0.200000
+ gain 1.000000
+ tau 0.160000
+ alfa 0.200000
+ drive_kangle 2.250000
+ drive_kdist 4.800000
+ w 0.260000
+ robotlength 0.265000
+ lim 0.200000
+ stopdist 0.180000
+ alarmdist 0.100000
+ velcmd 0.300000
+ acccmd 0.300000
+ nolinedist 0.200000
+
+Module : motor control
+ velscalel 100.000000
+ velscaler 100.000000
+ kp 66.000000
+ ki 10.000000
+
+Module : line sensor
+ size 8
+ k_filt 0.750000
+
+Module : filter
+ measurementnoise_std_x 0.100000
+ measurementnoise_std_y 0.110000
+ processnoise_std_stearingangle 0.010000
+ processnoise_std_dist 0.020000
+ type EKF
+ run 1
+ use 0
+ size_input 2
+ size_state 3
+ size_noise_process 2
+ size_output 2
+ size_noise_output 2
+ gpsoffset_easting 0.000000
+ gpsoffset_northing 0.000000
+
+Module : gpsmouse
+ hostname localhost
+ port 9500
+ SBAS 0
+ run 1
+ use 1
+
diff --git a/bash_src/laser.sh b/bash_src/laser.sh
new file mode 100644
index 0000000000000000000000000000000000000000..28163f44e387d4945e084598b522343cf44afe02
--- /dev/null
+++ b/bash_src/laser.sh
@@ -0,0 +1,4 @@
+#!usr/bin/bash
+# start the laser server
+cd ..
+ulmsserver
diff --git a/bash_src/log b/bash_src/log
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/bash_src/log_qclient/.PATH.TEST b/bash_src/log_qclient/.PATH.TEST
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/bash_src/log_ulmsserver/.PATH.TEST b/bash_src/log_ulmsserver/.PATH.TEST
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/bash_src/login.sh b/bash_src/login.sh
new file mode 100644
index 0000000000000000000000000000000000000000..36d8327df41d8a50e27f8cdd8b15d46e1594535f
--- /dev/null
+++ b/bash_src/login.sh
@@ -0,0 +1,2 @@
+#!usr/bin/bash
+sshfs -o ssh_command='ssh -J s192230@login.gbar.dtu.dk' k388h16@tustin.iau.dtu.dk:. /home/smr/k388
diff --git a/bash_src/logout.sh b/bash_src/logout.sh
new file mode 100644
index 0000000000000000000000000000000000000000..a3652f049ed3d022f6769297ffb2cebdbc8d1c93
--- /dev/null
+++ b/bash_src/logout.sh
@@ -0,0 +1,3 @@
+#!usr/bin/bash
+
+fusermount -u /home/smr/k388
diff --git a/bash_src/mov.sh b/bash_src/mov.sh
new file mode 100644
index 0000000000000000000000000000000000000000..ec16e0667ab727a130a7fa6d5b9415d39094c7a8
--- /dev/null
+++ b/bash_src/mov.sh
@@ -0,0 +1,9 @@
+#!usr/bin/bash
+# run mrc scripts
+cd ../mrc_src
+## create map
+mrc -s8000 createWalls
+mrc -s8000 spawnNodes
+wait 1
+## run the robot
+mrc -s8000 main
diff --git a/bash_src/sim.sh b/bash_src/sim.sh
new file mode 100644
index 0000000000000000000000000000000000000000..ad62001f4699e80c866ed6a6477142e67e494334
--- /dev/null
+++ b/bash_src/sim.sh
@@ -0,0 +1,4 @@
+#!usr/bin/bash
+# run the simulator
+cd ..
+simserver1 final.xml
diff --git a/bash_src/ulmsserver.ini b/bash_src/ulmsserver.ini
new file mode 100644
index 0000000000000000000000000000000000000000..d24097aa5260018a632b9332140c4508f06b9fe9
--- /dev/null
+++ b/bash_src/ulmsserver.ini
@@ -0,0 +1,32 @@
+# script to load URG and simulated laser scanner module
+server imagepath="."
+server datapath="."
+server replayPath="foo"
+module load="laserpool"
+module load="odopose"
+# other support modules
+module load="mapPose"
+module load="utmPose"
+module load="var"
+module load="aupoly.so.0"
+module load="mapbase.so.0"
+# define laserscanner to use
+scanset devtype=replay devname="laser_0.log"
+scanset devtype=urg devname="/dev/ttyACM0"
+scanset def=urg
+scanset mirror=true
+scanset width=180
+# uncomment next 2 lines to use fake laserscanner (in simulated environment)
+; scanset devtype=fake devname="urg"
+; scanset def=fake
+# set scanner position in robot coordinates
+scanset x=0.2 y=0.0 z=0.05
+scanget
+# localizer
+module load="aulocalize.so.0"
+; init localizer
+setInitPose x=0 y=0 th=0
+setInitCov Cx=0.04 Cy=0.04 Cth=0.04
+addline polyline="mapWall*"
+
+
diff --git a/calib/data.debug b/calib/data.debug
new file mode 100644
index 0000000000000000000000000000000000000000..9918c54dcbccca018f18ff2efe65184dd787ebf5
--- /dev/null
+++ b/calib/data.debug
@@ -0,0 +1,74 @@
+Debug file for the robot initialization
+---------------------------------------
+Tue Feb 18 16:04:04 2020
+---------------------------------------
+
+Module : robotinfo
+ type differential enum number 0
+ name smr
+
+Module : odometry
+ cl 0.000102
+ cr 0.000102
+ w 0.260000
+ robotlength 0.265000
+ ts 0.010000
+ maxtick 1000
+ control 0
+ enctype 0
+
+Module : motion control
+ ts 0.010000
+ line_gain 0.050000
+ line_tau 10.000000
+ line_alfa 0.300000
+ wall_gain 1.000000
+ wall_tau 0.700000
+ wall_alfa 0.200000
+ gain 1.000000
+ tau 0.160000
+ alfa 0.200000
+ drive_kangle 2.250000
+ drive_kdist 4.800000
+ w 0.260000
+ robotlength 0.265000
+ lim 0.200000
+ stopdist 0.180000
+ alarmdist 0.100000
+ velcmd 0.300000
+ acccmd 0.300000
+ nolinedist 0.200000
+
+Module : motor control
+ velscalel 100.000000
+ velscaler 100.000000
+ kp 66.000000
+ ki 10.000000
+
+Module : line sensor
+ size 8
+ k_filt 0.750000
+
+Module : filter
+ measurementnoise_std_x 0.100000
+ measurementnoise_std_y 0.110000
+ processnoise_std_stearingangle 0.010000
+ processnoise_std_dist 0.020000
+ type EKF
+ run 1
+ use 0
+ size_input 2
+ size_state 3
+ size_noise_process 2
+ size_output 2
+ size_noise_output 2
+ gpsoffset_easting 0.000000
+ gpsoffset_northing 0.000000
+
+Module : gpsmouse
+ hostname localhost
+ port 9500
+ SBAS 0
+ run 1
+ use 1
+
diff --git a/calib/localhost_demo_ls_calib.dat b/calib/localhost_demo_ls_calib.dat
new file mode 100644
index 0000000000000000000000000000000000000000..6f622683f09aa512e5ecd204c8b226ffbc46fd15
--- /dev/null
+++ b/calib/localhost_demo_ls_calib.dat
@@ -0,0 +1,16 @@
+255.00 ;max_white(1)
+255.00 ;max_white(2)
+255.00 ;max_white(3)
+255.00 ;max_white(4)
+255.00 ;max_white(5)
+255.00 ;max_white(6)
+255.00 ;max_white(7)
+255.00 ;max_white(8)
+0.00 ;max_black(1)
+0.00 ;max_black(2)
+0.00 ;max_black(3)
+0.00 ;max_black(4)
+0.00 ;max_black(5)
+0.00 ;max_black(6)
+0.00 ;max_black(7)
+0.00 ;max_black(8)
diff --git a/calib/robot.conf b/calib/robot.conf
new file mode 100644
index 0000000000000000000000000000000000000000..4c5f1d524958f212e6d6f71265cd0bf25ba793e3
--- /dev/null
+++ b/calib/robot.conf
@@ -0,0 +1,143 @@
+<?xml version="1.0" ?>
+
+<!--
+Configuration file for SMRdemo
+This file is for the SMR
+
+Robot types
+differential, ackerman
+
+Name
+smr, mmr, ackerbot, hako
+-->
+
+<robotinfo
+ type="differential"
+ name="smr"
+>
+ <debug/>
+ <speak />
+
+
+ <odometry
+ cl ="0.00010245"
+ cr ="0.00010245"
+ w ="0.26"
+ ts ="0.01"
+ counterwidth="16"
+ maxtick ="1000"
+ control ="0"
+ />
+
+ <motioncontrol
+ ts ="0.01"
+ line_gain ="0.05"
+ line_tau ="10"
+ line_alfa ="0.3"
+ wall_gain ="1"
+ wall_tau ="0.7"
+ wall_alfa ="0.2"
+ drive_kdist ="4.8"
+ drive_kangle="2.25"
+ kp ="10"
+ gain ="1"
+ tau ="0.16"
+ alfa ="0.2"
+ w ="0.26"
+ lim ="0.2"
+ stopdist ="0.18"
+ alarmdist ="0.1"
+ velcmd ="0.3"
+ acccmd ="0.3"
+ nolinedist ="0.2"
+ />
+
+ <motorcontrol
+ velscalel = "100"
+ velscaler ="100"
+ kp ="66"
+ ki ="10"
+ />
+
+ <linesensor
+ size ="8"
+ k_filt ="0.75"
+ />
+
+ <irsensor
+ />
+ <laserserver
+ hostname ="127.0.0.1"
+ port ="24919"
+ run ="1"
+ use ="1"
+ />
+
+
+ <cameraserver
+ hostname ="127.0.0.1"
+ port ="24920"
+ run ="1"
+ use ="1"
+ />
+
+ <gpsmouse
+ hostname ="localhost"
+ port ="9500"
+ SBAS ="0"
+ run ="1"
+ use ="1"
+ />
+ <!--
+ Information to connect to the Sensor Fusion
+ kalman filter. Minimum configuration.
+ type: Specifies the filter type, but is
+ currently not used.
+ run: Flag to indicate if the filter should run
+ use: Flag to indicate if the estimate should be used
+ measurements_std_x: Standard deviation on GPS noise easting
+ measurements_std_y: Standard deviation on GPS noise northing
+ processnoise_std_angle: Standard deviation on angle noise
+ processnoise_std_dist: Standard deviation on driven distance
+
+ gpsoffset_easting: Parameter to be used for indicating approximate
+ position of GPS
+ gpsoffset_northing: Parameter to be used for indicating approximate
+ position of GPS
+ -->
+
+ <filter
+ type="EKF"
+ run="1"
+ use="0"
+
+ size_input="2"
+ size_state="3"
+ size_noise_process="2"
+
+ size_output="2"
+ size_noise_output="2"
+
+ measurementnoise_std_x="0.1"
+ measurementnoise_std_y="0.11"
+ processnoise_std_stearingangle="0.01"
+ processnoise_std_dist="0.02"
+ >
+ <matrix name="A" size="3 3" type="double">
+ 1 2 3
+ 4 5 6
+ 7 8 9
+ </matrix>
+ <matrix name="B" size="3 3" type="double">
+ 12 22 32
+ 42 52 62
+ 72 82 92
+ </matrix>
+ <matrix name="C" size="3 3" type="double">
+ 14 24 34
+ 44 54 64
+ 74 84 94
+ </matrix>
+ </filter>
+
+</robotinfo>
diff --git a/calib/robotenv_demo_ls_calib.dat b/calib/robotenv_demo_ls_calib.dat
new file mode 100644
index 0000000000000000000000000000000000000000..6f622683f09aa512e5ecd204c8b226ffbc46fd15
--- /dev/null
+++ b/calib/robotenv_demo_ls_calib.dat
@@ -0,0 +1,16 @@
+255.00 ;max_white(1)
+255.00 ;max_white(2)
+255.00 ;max_white(3)
+255.00 ;max_white(4)
+255.00 ;max_white(5)
+255.00 ;max_white(6)
+255.00 ;max_white(7)
+255.00 ;max_white(8)
+0.00 ;max_black(1)
+0.00 ;max_black(2)
+0.00 ;max_black(3)
+0.00 ;max_black(4)
+0.00 ;max_black(5)
+0.00 ;max_black(6)
+0.00 ;max_black(7)
+0.00 ;max_black(8)
diff --git a/calib/smr6_demo_ls_calib.dat b/calib/smr6_demo_ls_calib.dat
new file mode 100644
index 0000000000000000000000000000000000000000..6000908d2b53a79cbd593176568d50ad12ed488f
--- /dev/null
+++ b/calib/smr6_demo_ls_calib.dat
@@ -0,0 +1,16 @@
+ 62.73 ;max_white(1)
+ 65.47 ;max_white(2)
+ 67.03 ;max_white(3)
+ 70.24 ;max_white(4)
+ 71.03 ;max_white(5)
+ 70.53 ;max_white(6)
+ 70.90 ;max_white(7)
+ 67.92 ;max_white(8)
+ 52.98 ;max_black(1)
+ 53.28 ;max_black(2)
+ 54.42 ;max_black(3)
+ 55.05 ;max_black(4)
+ 54.87 ;max_black(5)
+ 54.49 ;max_black(6)
+ 56.04 ;max_black(7)
+ 57.22 ;max_black(8)
diff --git a/calib/smrxqptest b/calib/smrxqptest
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/data.debug b/data.debug
new file mode 100644
index 0000000000000000000000000000000000000000..c5501aabb0226af48c5da5684f3ed032461c01b0
--- /dev/null
+++ b/data.debug
@@ -0,0 +1,74 @@
+Debug file for the robot initialization
+---------------------------------------
+Fri May 22 11:39:31 2020
+---------------------------------------
+
+Module : robotinfo
+ type differential enum number 0
+ name smr
+
+Module : odometry
+ cl 0.000102
+ cr 0.000102
+ w 0.260000
+ robotlength 0.265000
+ ts 0.010000
+ maxtick 1000
+ control 0
+ enctype 0
+
+Module : motion control
+ ts 0.010000
+ line_gain 0.050000
+ line_tau 10.000000
+ line_alfa 0.300000
+ wall_gain 1.000000
+ wall_tau 0.700000
+ wall_alfa 0.200000
+ gain 1.000000
+ tau 0.160000
+ alfa 0.200000
+ drive_kangle 2.250000
+ drive_kdist 4.800000
+ w 0.260000
+ robotlength 0.265000
+ lim 0.200000
+ stopdist 0.180000
+ alarmdist 0.100000
+ velcmd 0.300000
+ acccmd 0.300000
+ nolinedist 0.200000
+
+Module : motor control
+ velscalel 100.000000
+ velscaler 100.000000
+ kp 66.000000
+ ki 10.000000
+
+Module : line sensor
+ size 8
+ k_filt 0.750000
+
+Module : filter
+ measurementnoise_std_x 0.100000
+ measurementnoise_std_y 0.110000
+ processnoise_std_stearingangle 0.010000
+ processnoise_std_dist 0.020000
+ type EKF
+ run 1
+ use 0
+ size_input 2
+ size_state 3
+ size_noise_process 2
+ size_output 2
+ size_noise_output 2
+ gpsoffset_easting 0.000000
+ gpsoffset_northing 0.000000
+
+Module : gpsmouse
+ hostname localhost
+ port 9500
+ SBAS 0
+ run 1
+ use 1
+
diff --git a/datalog b/datalog
new file mode 100644
index 0000000000000000000000000000000000000000..4e6853c82360c70f888950ba715abdb5dfacbfee
--- /dev/null
+++ b/datalog
@@ -0,0 +1,12 @@
+laser "scanpush cmd='zoneobst'"
+%drive @v 0.2 :($drivendist>1)
+%stop
+log "$l0" "$l1" "$l2" "$l3" "$l4" "$l5" "$l6" "$l7" "$l8" :($l1>0)
+wait 3
+turn 30
+log "$l0" "$l1" "$l2" "$l3" "$l4" "$l5" "$l6" "$l7" "$l8"
+wait 3
+turn 20
+log "$l0" "$l1" "$l2" "$l3" "$l4" "$l5" "$l6" "$l7" "$l8"
+wait 3
+
diff --git a/final.xml b/final.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0c76f6427ae22c675ebc9e900c68fbe1d4862f2e
--- /dev/null
+++ b/final.xml
@@ -0,0 +1,78 @@
+<?xml version="1.0" ?>
+<!--
+ Configuration file for
+ Simulator Server
+-->
+
+<simserver>
+ <!-- *** Core Components Configuration *** -->
+ <!-- Sheduler configuration -->
+ <scheduler>
+ <period value="10000"/><!--in usec-->
+ </scheduler>
+
+ <!-- Simulator configuration -->
+ <simstage>
+ <world wallmap="388project" linemap="" scale="150" yoff="775"/>
+ 1<guidemark gmno="1" gmpose=" 0 1.5 3.14 "/>
+ 2<guidemark gmno="7" gmpose=" 0 1.5 0"/>
+ 3<guidemark gmno="6" gmpose=" 4.0 1.5 3.14 "/>
+ 4<guidemark gmno="4" gmpose=" 4.0 1.5 0"/>
+ 5<guidemark gmno="5" gmpose=" 0.0 3.5 3.14"/>
+ 6<guidemark gmno="12" gmpose=" 0.0 3.5 0"/>
+ 7<guidemark gmno="11" gmpose=" 4.0 3.5 3.14"/>
+ 8<guidemark gmno="5" gmpose=" 4.0 3.5 0.0"/>
+ 9<guidemark gmno="5" gmpose=" 0.0 4.5 0.0"/>
+ 10<guidemark gmno="5" gmpose=" 4.0 4.5 3.14"/>
+ 11<guidemark gmno="3" gmpose=" 0.5 5.0 -1.57"/>
+ 12<guidemark gmno="13" gmpose=" 3.5 5.0 -1.57"/>
+ 13<guidemark gmno="98" gmpose=" 2.0 4.0 3.14"/>
+ 14<guidemark gmno="98" gmpose=" 2.0 4.0 0"/>
+ </simstage>
+
+ <!-- Robot server no. 0 -->
+ <!-- Robot size: 0.35*0.3 -->
+ <robot>
+ <mrcserver port="8000"/>
+ <clients number="5"/>
+ <params name="smr0" robotpose="0.25 0.45 0" wheelbase="0.26" encresolution="0.00010245"/>
+ <corner x="-0.1" y="-0.15"/>
+ <corner x="0.25" y="-0.15"/>
+ <corner x="0.25" y="0.15"/>
+ <corner x="-0.1" y="0.15"/>
+ <irsensor ka="16.0" kb="76.0"/>
+ <laserscanner port="19001" laserpose="0.26 0.0 0.0"/>
+ </robot>
+
+ <!-- Robot server no. 1
+ <robot>
+ <mrcserver port="8001"/>
+ <clients number="5"/>
+ <params name="smr1" robotpose=" 1.0 1.0 0.0" wheelbase="0.26" encresolution="0.00010245"/>
+ <corner x="-0.1" y="-0.15"/>
+ <corner x="0.25" y="-0.15"/>
+ <corner x="0.25" y="0.15"/>
+ <corner x="-0.1" y="0.15"/>
+ <irsensor ka="16.0" kb="76.0"/>
+ <laserscanner port="19001" laserpose="0.26 0 0"/>
+ </robot>
+ -->
+ <!-- Robot server no. 2
+ <robot>
+ <mrcserver port="8002"/>
+ <clients number="4"/>
+ <params name="smr2" wheelbase="0.26" encresolution="0.00010245"/>
+ <irsensor ka="16.0" kb="76.0"/>
+ <laserscanner port="19002"/>
+ </robot>
+-->
+ <!-- Robot server no. 3
+ <robot>
+ <mrcserver port="8003"/>
+ <clients number="5"/>
+ <params name="smr3" wheelbase="0.26" encresolution="0.00010245"/>
+ <irsensor ka="16.0" kb="76.0"/>
+ <laserscanner port="19003"/>
+ </robot>
+-->
+</simserver>
diff --git a/final_project_final b/final_project_final
new file mode 100644
index 0000000000000000000000000000000000000000..b272e75697da2bbdd751df4e1d5abc2a24ba0811
--- /dev/null
+++ b/final_project_final
@@ -0,0 +1,693 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%FINAL PROJECT%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%31388%%%%%%%%%%%%%%%%%%%
+%%%%%%%%Advanced Autonomous Robots%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%Dennis Jensen, s155629%%%%%%%%%
+%%%%%%%%%%%Martin Jensen, s164033%%%%%%%%%
+%%%%%%%%%%%Theis Jensen, s164022%%%%%%%%%
+%%%%%%%%%%%Stefan Larsen, s164029%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% laser update call
+laser "resetplanner"
+laser "scanget"
+laser "resetlocalizer"
+
+% For logging purposes
+laser "scanset logopen"
+laser "scanset log=1"
+laser "odopose log=true"
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%MISSION%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% mission 1: follow guidemarks
+% mission 2: find object
+% mission 3: drive to goal
+
+%first mission is initialized
+mission = 1
+
+%first guidemark to visit
+gmkno = 2
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%CONSTANTS%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fwdVel = 1.0 %forward velocity
+slowVel = 0.5 %slow forward velocity
+
+pi = 3.141592
+
+xlaser = 0.26 %laser offset in x-direction
+
+
+
+%-------------------
+% POINTS
+%-------------------
+% straight up from start position
+laser "addpoint pno=1 x=0.5 y=0.5"
+laser "addpoint pno=2 x=0.5 y=1.5"
+laser "addpoint pno=3 x=0.5 y=2.35"
+laser "addpoint pno=4 x=0.5 y=3.5"
+laser "addpoint pno=5 x=0.5 y=4.55"
+
+% top left straight right (Inside track)
+laser "addpoint pno=6 x=1.5 y=4.65"
+laser "addpoint pno=7 x=2.5 y=4.65"
+laser "addpoint pno=8 x=3.5 y=4.55"
+
+% Right corner and down (inside track)
+laser "addpoint pno=9 x=3.5 y=3.5"
+laser "addpoint pno=10 x=3.5 y=2.35"
+laser "addpoint pno=11 x=3.5 y=1.5"
+laser "addpoint pno=12 x=3.5 y=0.5"
+
+% Right corner bottom back to start
+laser "addpoint pno=13 x=2.5 y=0.5"
+laser "addpoint pno=14 x=1.5 y=0.5"
+
+% Left side out of track box
+laser "addpoint pno=15 x=-0.5 y=2.5"
+laser "addpoint pno=16 x=-0.5 y=3.5"
+laser "addpoint pno=17 x=-0.5 y=1.5"
+
+% Right side out of track box
+laser "addpoint pno=18 x=4.5 y=2.5"
+laser "addpoint pno=19 x=4.5 y=3.5"
+laser "addpoint pno=20 x=4.5 y=1.5"
+
+% From left side to the entrance of the red box (G13, G14)
+laser "addpoint pno=21 x=1.25 y=2.35"
+
+% Infront of the entrance of the red box
+laser "addpoint pno=22 x=2.0 y=2.25"
+
+% From right side to the entrance of the red box (G13, G14)
+laser "addpoint pno=23 x=2.75 y=2.35"
+
+% Inside the red box (G13, G14)
+%laser "addpoint pno=24 x=2 y=3"
+laser "addpoint pno=25 x=2 y=3.3"
+%% Inside the red box (G13)
+laser "addpoint pno=26 x=1.25 y=3.4"
+laser "addpoint pno=27 x=1.25 y=4.0"
+%%% Infront of G13
+laser "addpoint pno=28 x=1.4 y=4.0"
+%% Inside the red box (G14)
+laser "addpoint pno=29 x=2.8 y=3.4"
+laser "addpoint pno=30 x=2.8 y=4.0"
+%%% Infront of G14
+laser "addpoint pno=31 x=2.7 y=4.0"
+
+%-------------------
+% CONNECTIONS
+%-------------------
+laser "addcon pno1=1 pno2=2"
+laser "addcon pno1=2 pno2=1"
+
+laser "addcon pno1=2 pno2=3"
+laser "addcon pno1=3 pno2=2"
+
+laser "addcon pno1=3 pno2=4"
+laser "addcon pno1=4 pno2=3"
+
+laser "addcon pno1=4 pno2=5"
+laser "addcon pno1=5 pno2=4"
+
+laser "addcon pno1=5 pno2=6"
+laser "addcon pno1=6 pno2=5"
+
+laser "addcon pno1=6 pno2=7"
+laser "addcon pno1=7 pno2=6"
+
+laser "addcon pno1=7 pno2=8"
+laser "addcon pno1=8 pno2=7"
+
+laser "addcon pno1=8 pno2=9"
+laser "addcon pno1=9 pno2=8"
+
+laser "addcon pno1=9 pno2=10"
+laser "addcon pno1=10 pno2=9"
+
+laser "addcon pno1=10 pno2=11"
+laser "addcon pno1=11 pno2=10"
+
+laser "addcon pno1=11 pno2=12"
+laser "addcon pno1=12 pno2=11"
+
+laser "addcon pno1=12 pno2=13"
+laser "addcon pno1=13 pno2=12"
+
+laser "addcon pno1=13 pno2=14"
+laser "addcon pno1=14 pno2=13"
+
+laser "addcon pno1=14 pno2=1"
+laser "addcon pno1=1 pno2=14"
+
+laser "addcon pno1=3 pno2=15"
+laser "addcon pno1=15 pno2=3"
+
+laser "addcon pno1=15 pno2=16"
+laser "addcon pno1=16 pno2=15"
+
+laser "addcon pno1=15 pno2=17"
+laser "addcon pno1=17 pno2=15"
+
+laser "addcon pno1=10 pno2=18"
+laser "addcon pno1=18 pno2=10"
+
+laser "addcon pno1=18 pno2=19"
+laser "addcon pno1=19 pno2=18"
+
+laser "addcon pno1=18 pno2=20"
+laser "addcon pno1=20 pno2=18"
+
+laser "addcon pno1=3 pno2=21"
+laser "addcon pno1=21 pno2=3"
+
+laser "addcon pno1=21 pno2=22"
+laser "addcon pno1=22 pno2=21"
+
+laser "addcon pno1=22 pno2=25"
+laser "addcon pno1=25 pno2=22"
+
+%laser "addcon pno1=24 pno2=25"
+%laser "addcon pno1=25 pno2=24"
+
+laser "addcon pno1=25 pno2=26"
+laser "addcon pno1=26 pno2=25"
+
+laser "addcon pno1=26 pno2=27"
+laser "addcon pno1=27 pno2=26"
+
+laser "addcon pno1=27 pno2=28"
+laser "addcon pno1=28 pno2=27"
+
+laser "addcon pno1=25 pno2=29"
+laser "addcon pno1=29 pno2=25"
+
+laser "addcon pno1=29 pno2=30"
+laser "addcon pno1=30 pno2=29"
+
+laser "addcon pno1=30 pno2=31"
+laser "addcon pno1=31 pno2=30"
+
+laser "addcon pno1=22 pno2=23"
+laser "addcon pno1=23 pno2=22"
+
+laser "addcon pno1=10 pno2=23"
+laser "addcon pno1=23 pno2=10"
+
+% calculate the cost
+laser "calculatecost"
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%ADDING LINES%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+laser "addline startx=0 starty=0 endx=1.8 endy=0 name='bottom left'"
+laser "addline startx=2.2 starty=0 endx=4.0 endy=0 name='ottom right'"
+laser "addline startx=0 starty=5.0 endx=1.8 endy=0 name='top left'"
+laser "addline startx=2.2 starty=5.0 endx=4.0 endy=5.0 name='top right'"
+laser "addline startx=0 starty=0 endx=0 endy=1.8 name='left down'"
+laser "addline startx=0 starty=3.2 endx=0 endy=5 name='left up'"
+laser "addline startx=4 starty=0 endx=4 endy=1.8 name='right down'"
+laser "addline startx=4 starty=3.2 endx=4 endy=5 name='right up'"
+laser "addline startx=0.9 starty=3.1 endx=1.7 endy=3.1 name='maze bottom left'"
+laser "addline startx=2.3 starty=3.1 endx=3.1 endy=3.1 name='maze bottom right'"
+laser "addline startx=1.7 starty=2.5 endx=1.7 endy=3.1 name='maze left down'"
+laser "addline startx=0.9 starty=3.1 endx=0.9 endy=4.3 name='maze left up'"
+laser "addline startx=2.3 starty=2.5 endx=2.3 endy=3.1 name='maze right down'"
+laser "addline startx=3.1 starty=3.1 endx=3.1 endy=4.3 name='maze right up'"
+laser "addline startx=1.5 starty=3.7 endx=2.5 endy=3.7 name='maze middle horizontal'"
+laser "addline startx=2.0 starty=3.7 endx=2.0 endy=4.3 name='maze middle vertical'"
+laser "addline startx=0.9 starty=4.3 endx=3.1 endy=4.3 name='maze top'"
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%SET UP%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+% start pose and covariance
+laser "setinitpose x=xlaser y=0 th=0"
+laser "setinitcov Cx=0.01 Cy=0.01 Cth=0.01"
+
+set "$odox" 0.25
+set "$odoy" 0.25
+set "$odoth" 0
+
+% set localizer
+laser "LOCALIZE silent"
+laser "push t='0.5' cmd='localize'"
+
+%-------------------
+% GUIDEMARKS
+%-------------------
+array "gmkx" 14
+array "gmky" 14
+array "gmkth" 14
+
+gmkx[1] = -0.01
+gmkx[2] = 0
+gmkx[3] = 4.0
+gmkx[4] = 4.01
+gmkx[5] = -0.01
+gmkx[6] = 0.0
+gmkx[7] = 4.0
+gmkx[8] = 4.01
+gmkx[9] = 0.0
+gmkx[10] = 4.0
+gmkx[11] = 0.5
+gmkx[12] = 3.5
+gmkx[13] = 1.99
+gmkx[14] = 2.01
+
+gmky[1] = 1.5
+gmky[2] = 1.5
+gmky[3] = 1.5
+gmky[4] = 1.5
+gmky[5] = 3.5
+gmky[6] = 3.5
+gmky[7] = 3.5
+gmky[8] = 3.5
+gmky[9] = 4.5
+gmky[10] = 4.5
+gmky[11] = 5.0
+gmky[12] = 5.0
+gmky[13] = 4.0
+gmky[14] = 4.0
+
+gmkth[1] = 3.14
+gmkth[2] = 0
+gmkth[3] = 3.14
+gmkth[4] = 0
+gmkth[5] = 3.14
+gmkth[6] = 0
+gmkth[7] = 3.14
+gmkth[8] = 0
+gmkth[9] = 0
+gmkth[10] = 3.14
+gmkth[11] = -1.57
+gmkth[12] = -1.57
+gmkth[13] = 3.14
+gmkth[14] = 0
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%RUN MISSION%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+label "start"
+
+switch(mission)
+ case 1
+ goto "mission1"
+ case 2
+ goto "mission2"
+ case 3
+ goto "mission3"
+endswitch
+
+goto "missionerror"
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%MISSION 1%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+label "mission1"
+
+call "init"
+call "drive"
+call "gm"
+
+if (gmval != 14) "mission1"
+
+goto "complete"
+
+label "init"
+
+%current pose
+xc = $odox
+yc = $odoy
+thc = $odoth
+
+%first guidemark pose
+xg = gmkx[gmkno]
+yg = gmky[gmkno]
+thg = gmkth[gmkno]
+
+stringcat "findroute startx="xc" starty="yc" endx="xg" endy="yg""
+laser "$string"
+wait 1
+N = $l4
+
+
+return
+
+
+
+%-------------------------%
+% Drive %
+%-------------------------%
+
+label "drive"
+
+stringcat "getpoint p=" N""
+laser "$string"
+wait 1
+
+x = $l5
+y = $l6
+th = $l7
+
+eval x
+eval y
+eval th
+
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+
+angle = th-$res2
+angle = normalizeanglerad(angle)
+ignoreobstacles
+eval angle
+wait 0.5
+turn angle "rad" @v 0.3 :($cmdtime>3)
+
+ignoreobstacles
+wait 0.5
+drivew x y th "rad" @v 0.8 :($targetdist<0.05)
+stop
+wait 1
+
+
+N = N - 1
+if (N>-1) "drive"
+
+
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+
+xx = xg-$res0
+yy = yg-$res1
+
+angle = atan2(yy,xx)-$res2
+
+wait 1
+
+angle = normalizeanglerad(angle)
+ignoreobstacles
+turn angle "rad" @v 0.3 :($cmdtime>3)
+wait 0.5
+
+
+return
+
+
+
+%------------------------%
+% DOING EVALUATION OF GM %
+%------------------------%
+
+label "gm"
+call "readgm"
+if (gmval < 1) "bad"
+if (gmval >= 1) "good"
+
+label "bad"
+
+wait 1
+gmval = $fiducialid
+
+if (gmval >= 1) "good"
+turn 10 @v 0.2
+wait 1
+call "readgm"
+if (gmval >= 1) "good"
+turn -20 @v 0.2
+wait 1
+call "readgm"
+if (gmval >= 1) "good"
+return
+
+label "good"
+gmkno = gmval
+stringcat "Guidemark no. " "gmkno""
+return
+
+
+
+%-------------------------%
+% READ GUIDEMARK FUNCTION %
+%-------------------------%
+
+label "readgm"
+tries = 0
+label "retry"
+wait 2 :($fiducialid)
+eval $fiducialid
+gmval = $fiducialid
+wait 2
+tries = tries + 1
+
+if (gmval == 98) "complete"
+
+if (tries < 2 | gmval < 1) "retry"
+
+return
+
+label "complete"
+mission = mission + 1
+goto "start"
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%MISSION 2%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% This mission finds the object on the loose
+label "mission2"
+
+n=1 %iterator for switch case
+
+% (x,y) of box with object on the loose
+midx = 2
+midy = 1.5
+
+% Go to reading points
+label "objectfinding"
+
+
+% Read from points 22 -> 21 -> 3 -> 2 -> 1 -> 14 -> 13 -> 12 -> 11 -> 10 -> 23
+switch(n)
+ case 1:
+ x=2.0
+ y=2.25
+ case 2:
+ x=1.25
+ y=2.35
+ case 3:
+ x=0.5
+ y=2.35
+ case 4:
+ x=0.5
+ y=1.5
+ case 5:
+ x=0.5
+ y=0.5
+ case 6:
+ x=1.5
+ y=0.5
+ case 7:
+ x=2.5
+ y=0.5
+ case 8:
+ x=3.5
+ y=0.5
+ case 9:
+ x=3.5
+ y=1.5
+ case 10:
+ x=3.5
+ y=2.35
+ case 11:
+ x=2.75
+ y=2.35
+endswitch
+
+
+xc = $odox
+yc = $odoy
+thc = $odoth
+
+
+stringcat "findroute startx="xc" starty="yc" endx="x" endy="y""
+laser "$string"
+wait 1
+
+K = $l4
+
+
+label "drivearound"
+stringcat "getpoint p=" K""
+laser "$string"
+wait 1
+
+x = $l5
+y = $l6
+
+th = $l7
+
+eval x
+eval y
+eval th
+
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+
+angle = th-$res2
+angle = normalizeanglerad(angle)
+eval angle
+ignoreobstacles
+turn angle "rad" @v 0.3 :($cmdtime>3)
+
+ignoreobstacles
+wait 0.5
+drivew x y th "rad" @v 0.5 :($targetdist<0.05)
+stop
+wait 0.5
+
+K=K-1
+
+if (K>-1) "drivearound"
+
+
+% Turning towards middle of box on the loose
+
+posx = midx - $odox
+posy = midy - $odoy
+
+midangle = atan2(posy, posx) - $odoth
+midangle = normalizeanglerad(midangle)
+
+ignoreobstacles
+turn midangle "rad" @v 0.2 :($cmdtime>3)
+wait 2
+stringcat "zoneobst detect x="$odox" y="$odoy" th="$odoth""
+laser "$string"
+wait 2
+
+n=n+1
+
+if (n!=12) "objectfinding"
+
+wait 1
+
+%determine object
+laser "zoneobst determine"
+wait 3
+
+goto "finish"
+
+mission = mission + 1
+goto "start"
+
+
+label "findobject"
+posx = midx - $odox
+posy = midy - $odoy
+
+midangle = atan2(posy, posx) - $odoth
+midangle = normalizeanglerad(midangle)
+
+ignoreobstacles
+turn midangle "rad" @v 0.2
+wait 2
+
+stringcat "zoneobst findobject x="$odox" y="$odoy" th="$odoth""
+laser "$string"
+
+boxx = $l0
+boxy = $l1
+
+eval boxx
+eval boxy
+
+return
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%MISSION 3%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% drive to goal
+
+label "mission3"
+
+startx=0.25
+starty=0.25
+
+xc = $odox
+yc = $odoy
+stringcat "findroute startx="xc" starty="yc" endx="startx" endy="starty""
+laser "$string"
+wait 1
+N = $l4
+
+label "driveHome"
+
+stringcat "getpoint p=" N""
+laser "$string"
+wait 1
+
+x = $l5
+y = $l6
+th = $l7
+
+eval x
+eval y
+eval th
+
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+
+angle = th-$res2
+angle = normalizeanglerad(angle)
+ignoreobstacles
+eval angle
+wait 1
+turn angle "rad" @v 0.3 :($cmdtime>3)
+
+ignoreobstacles
+wait 1
+drivew x y th "rad" @v 0.8 :($targetdist<0.05)
+stop
+wait 1
+
+N = N - 1
+
+if (N>-1) "driveHome"
+
+turn 90
+fwd -0.25
+turn 90
+fwd -0.25
+
+goto "finish"
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%FINISH%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+label "missionerror"
+%NO MISSION NUMBER DETECTED
+
+label "finish"
+stop
+
+% closing logging
+laser "scanset logclose"
+laser "odopose log=false"
+
+% DRIVE FINISHED
diff --git a/foo.png b/foo.png
new file mode 100644
index 0000000000000000000000000000000000000000..a45d31d1bc0b567385eaeaaa47b15dd3271fb022
Binary files /dev/null and b/foo.png differ
diff --git a/formerSrc/data.debug b/formerSrc/data.debug
new file mode 100644
index 0000000000000000000000000000000000000000..355524c1f63b4e3030f84b88cb168f8bd6c2f9ad
--- /dev/null
+++ b/formerSrc/data.debug
@@ -0,0 +1,74 @@
+Debug file for the robot initialization
+---------------------------------------
+Wed Apr 15 11:44:27 2020
+---------------------------------------
+
+Module : robotinfo
+ type differential enum number 0
+ name smr
+
+Module : odometry
+ cl 0.000102
+ cr 0.000102
+ w 0.260000
+ robotlength 0.265000
+ ts 0.010000
+ maxtick 1000
+ control 0
+ enctype 0
+
+Module : motion control
+ ts 0.010000
+ line_gain 0.050000
+ line_tau 10.000000
+ line_alfa 0.300000
+ wall_gain 1.000000
+ wall_tau 0.700000
+ wall_alfa 0.200000
+ gain 1.000000
+ tau 0.160000
+ alfa 0.200000
+ drive_kangle 2.250000
+ drive_kdist 4.800000
+ w 0.260000
+ robotlength 0.265000
+ lim 0.200000
+ stopdist 0.180000
+ alarmdist 0.100000
+ velcmd 0.300000
+ acccmd 0.300000
+ nolinedist 0.200000
+
+Module : motor control
+ velscalel 100.000000
+ velscaler 100.000000
+ kp 66.000000
+ ki 10.000000
+
+Module : line sensor
+ size 8
+ k_filt 0.750000
+
+Module : filter
+ measurementnoise_std_x 0.100000
+ measurementnoise_std_y 0.110000
+ processnoise_std_stearingangle 0.010000
+ processnoise_std_dist 0.020000
+ type EKF
+ run 1
+ use 0
+ size_input 2
+ size_state 3
+ size_noise_process 2
+ size_output 2
+ size_noise_output 2
+ gpsoffset_easting 0.000000
+ gpsoffset_northing 0.000000
+
+Module : gpsmouse
+ hostname localhost
+ port 9500
+ SBAS 0
+ run 1
+ use 1
+
diff --git a/formerSrc/ex11 b/formerSrc/ex11
new file mode 100644
index 0000000000000000000000000000000000000000..5d68f805b7a9a9a39544cbf7e914002d941ea89d
--- /dev/null
+++ b/formerSrc/ex11
@@ -0,0 +1,8 @@
+laser "addline startx=0 starty=0 endx=1.8 endy=0 name='line1'"
+laser "addline startx=1.8 starty=0 endx=1.8 endy=1.8 name='line2'"
+laser "addline startx=1.8 starty=1.8 endx=0 endy=1.8 name='line3'"
+laser "addline startx=0 starty=1.8 endx=0 endy=0 name='line4'"
+laser "setinitpose x=0.95 y=1 th=0"
+laser "setinitcov Cx=1 Cy=1 Cth=1"
+laser "localize"
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
diff --git a/formerSrc/ex11-1 b/formerSrc/ex11-1
new file mode 100644
index 0000000000000000000000000000000000000000..815cda2981f450ba9d115c9ea3d5eed10abe2456
--- /dev/null
+++ b/formerSrc/ex11-1
@@ -0,0 +1,11 @@
+laser "localize"
+wait 1
+wait 1
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+wait 2
+eval $l0
+eval $l1
+eval $l2
+eval $res0
+eval $res1
+eval $res2
diff --git a/formerSrc/ex11-2 b/formerSrc/ex11-2
new file mode 100644
index 0000000000000000000000000000000000000000..6cc6a8ca1a03a83d2ab83aa90d319e585f03db64
--- /dev/null
+++ b/formerSrc/ex11-2
@@ -0,0 +1,26 @@
+% since drive is based on odometry
+% the start point is (0,0,0), in simulator it is set (0.9, 0.9, 0)
+% go to the first corner
+ignoreobstacles
+fwd 0.5
+ignoreobstacles
+turn 90
+ignoreobstacles
+fwd 0.5
+ignoreobstacles
+turn 90
+% N is the counter
+% Now we arrive the first point
+N=0
+label "loop"
+drive -0.5 0.5 180: ($targetdist <0.25)
+wait 1
+drive -0.5 -0.5 270: ($targetdist <0.25)
+wait 1
+drive 0.5 -0.5 0: ($targetdist <0.25)
+wait 1
+drive 0.5 0.5 90: ($targetdist <0.25)
+wait 1
+N = N+1
+wait 1
+if(N<5) "loop"
diff --git a/formerSrc/ex11-3 b/formerSrc/ex11-3
new file mode 100644
index 0000000000000000000000000000000000000000..86f7d8dbfc4bc6cbedf2667cd9df917c184ba53a
--- /dev/null
+++ b/formerSrc/ex11-3
@@ -0,0 +1,19 @@
+% drive the robot via Kalman Filter
+laser "localize"
+laser “push t='1.0' cmd='localize' “
+wait 1
+N=0
+ignoreobstacles
+fwd 0.5
+wait 1
+label "loop"
+drivew 1.4 1.4 90 :($targetdist<0.25)
+wait 3
+drivew 0.4 1.4 180 :($targetdist<0.25)
+wait 3
+drivew 0.4 0.4 270 :($targetdist<0.25)
+wait 3
+drivew 1.4 0.4 0 :($targetdist<0.25)
+wait 3
+N = N+1
+if (N<9) "loop"
diff --git a/formerSrc/ex11-4 b/formerSrc/ex11-4
new file mode 100644
index 0000000000000000000000000000000000000000..2ac128c6045f39c013c9a4d1577f6be7357d53aa
--- /dev/null
+++ b/formerSrc/ex11-4
@@ -0,0 +1,45 @@
+% drive the robot via Kalman Filter
+laser "resetplanner"
+set "$odox" 0
+set "$odoy" 0
+set "$odoth" 0
+laser "localize"
+laser “push t='1.0' cmd='localize'“
+wait 3
+yh = 1.4
+yl = 1.2
+ignoreobstacles
+fwd 0.5
+wait 1
+drivew 1.4 yh 90:($targetdist<0.25)
+wait 3
+label "loop"
+call "move"
+yh = yh-0.4
+yl = yl-0.4
+wait 1
+if (yl<0.2) "stop"
+goto "loop"
+label "move"
+drivew 0.4 yh 180:($targetdist<0.25)
+wait 3
+turn 90
+fwd 0.2
+wait 3
+%drivew 0.4 yl 270:($targetdist<0.25)
+%wait 3
+drivew 1.4 yl 0:($targetdist<0.25)
+wait 3
+turn -90
+fwd 0.2
+turn 180
+wait 3
+return
+label "show"
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+wait 1
+eval $res0
+eval $res1
+eval $res2
+return
+label "stop"
\ No newline at end of file
diff --git a/formerSrc/ex12 b/formerSrc/ex12
new file mode 100644
index 0000000000000000000000000000000000000000..894850f0a853e9744402c248cf3f509764c528a2
--- /dev/null
+++ b/formerSrc/ex12
@@ -0,0 +1,22 @@
+drive -0.5 0.5 135:($targetdist<0.25)
+drive -1 1 135:($targetdist<0.25)
+drive -1.5 1.5 135:($targetdist<0.25)
+drive -2 1.5 180:($targetdist<0.25)
+drive -2.5 1.5 180:($targetdist<0.25)
+drive -3 2 135:($targetdist<0.25)
+drive -2.5 3 45:($targetdist<0.25)
+ignoreobstacles
+drive -2 3.5 45:($targetdist<0.25)
+ignoreobstacles
+drive -1.5 3.5 0:($targetdist<0.25)
+drive -1 3.5 0:($targetdist<0.25)
+drive -0.5 4 45:($targetdist<0.25)s
+drive -1 5 135:($targetdist<0.25)
+ignoreobstacles
+drive -1.5 5.5 135:($targetdist<0.25)
+drive -2 5.5 180:($targetdist<0.25)
+drive -2.5 5.5 180:($targetdist<0.25)
+drive -3 5.5 180:($targetdist<0.25)
+drive -3.5 5.5 180:($targetdist<0.25)
+drive -4 5.5 180
+stop
\ No newline at end of file
diff --git a/formerSrc/ex13-1 b/formerSrc/ex13-1
new file mode 100644
index 0000000000000000000000000000000000000000..38f93193d6a9cabf0ba406c8b06bb88479b7c498
--- /dev/null
+++ b/formerSrc/ex13-1
@@ -0,0 +1,13 @@
+laser "findroute startx=3.2 starty=0.3 endx=0.3 endy=3.3"
+wait 1
+c = $l4
+eval c
+label "loop"
+stringcat "getpoint p="c
+laser "$string"
+wait 1
+eval $l5
+eval $l6
+eval $l7
+c=c-1
+if (c>-1) "loop"
\ No newline at end of file
diff --git a/formerSrc/ex13-2 b/formerSrc/ex13-2
new file mode 100644
index 0000000000000000000000000000000000000000..e168db7e9a4adf632cefc822308337a1fc791ecb
--- /dev/null
+++ b/formerSrc/ex13-2
@@ -0,0 +1,57 @@
+%%%%%%%%%%%%%
+%% Kalman localization
+laser "localize"
+laser "push t='1.0' cmd='localize'"
+wait 1
+%% set odometry
+%set "$odox" 3.3
+%set "$odoy" 0.35
+%set "$odoth" 0
+%% make a turn
+turn 180
+wait 1
+%% make an array to store points
+array "x" 10
+array "y" 10
+array "th" 10
+x[0]=1.7
+x[1]=0.2
+x[2]=0.2
+x[3]=0.95
+x[4]=0.95
+x[5]=0.2
+x[6]=0.2
+x[7]=1.7
+x[8]=1.7
+x[9]=0.2
+y[0]=0.3
+y[1]=0.3
+y[2]=1.05
+y[3]=1.05
+y[4]=1.8
+y[5]=1.8
+y[6]=2.55
+y[7]=2.55
+y[8]=3.3
+y[9]=3.3
+th[0]=180
+th[1]=90
+th[2]=0
+th[3]=90
+th[4]=180
+th[5]=90
+th[6]=0
+th[7]=90
+th[8]=180
+th[9]=180
+%% run route
+c = 0
+label "loop"
+xx = x[c]
+yy = y[c]
+t = th[c]
+drivew xx yy t:($targetdist<0.25)
+wait 3
+c=c+1
+eval c
+if(c<10) "loop"
\ No newline at end of file
diff --git a/formerSrc/ex13-3-1 b/formerSrc/ex13-3-1
new file mode 100644
index 0000000000000000000000000000000000000000..5883ca29c0e839cc2c61f3ed06a4ac923b22e5d2
--- /dev/null
+++ b/formerSrc/ex13-3-1
@@ -0,0 +1,27 @@
+% find route
+laser "findroute startx=3.3 starty=0.3 endx=0.38 endy=3.38"
+wait 1
+%%%%%%%%%%%%%
+%% Kalman localization
+laser "localize"
+laser "push t='1.0' cmd='localize'"
+wait 5
+%% set odometry
+%set "$odox" 3.3
+%set "$odoy" 0.35
+%set "$odoth" 0
+%% run route
+c = $l4
+eval c
+label "loop"
+stringcat "getpoint p="c
+laser "$string"
+wait 1
+eval $l5
+eval $l6
+eval $l7
+th = $l7/3.14159*180
+drivew $l5 $l6 th:($targetdist<0.25)
+wait 3
+c=c-1
+if (c>-1) "loop"
\ No newline at end of file
diff --git a/formerSrc/ex13-3-2 b/formerSrc/ex13-3-2
new file mode 100644
index 0000000000000000000000000000000000000000..4b1c918ab85d108a94f034c8e3efc807e07aa69a
--- /dev/null
+++ b/formerSrc/ex13-3-2
@@ -0,0 +1,53 @@
+%%%%%%%%%%%%%
+%% Kalman localization
+laser "localize"
+laser "push t='1.0' cmd='localize'"
+wait 3
+%% set odometry
+%set "$odox" 1.7
+%set "$odoy" 0.3
+%set "$odoth" 0
+%%%%%%%%%%%
+% find route
+array "sx" 5
+array "sy" 5
+id = 0
+sx[0]=1.7
+sx[1]=0.2
+sx[2]=3.2
+sx[3]=0.2
+sx[4]=1.7
+sy[0]=0.3
+sy[1]=0.3
+sy[2]=0.3
+sy[3]=1.05
+sy[4]=0.3
+label "bigloop"
+startx=sx[id]
+starty=sy[id]
+endx=sx[id+1]
+endy=sy[id+1]
+eval startx
+eval starty
+eval endx
+eval endy
+stringcat "findroute startx="startx"starty="starty"endx="endx"endy="endy
+laser "$string"
+wait 1
+%% run route
+c = $l4
+eval c
+label "loop"
+stringcat "getpoint p="c
+laser "$string"
+wait 1
+eval $l5
+eval $l6
+eval $l7
+th = $l7/3.14159*180
+drivew $l5 $l6 th:($targetdist<0.25)
+wait 5
+c=c-1
+if (c>-1) "loop"
+id = id+1
+if (id<4) "bigloop"
\ No newline at end of file
diff --git a/formerSrc/feedBackControl b/formerSrc/feedBackControl
new file mode 100644
index 0000000000000000000000000000000000000000..2467985e5958dcfada71ac7cfd0050857c2ae0b0
--- /dev/null
+++ b/formerSrc/feedBackControl
@@ -0,0 +1,49 @@
+% feed back control
+%%%%%%%%%%%
+xTw = 0.1
+yTw = 0.1
+thTw = 90/180*3.1415926535 %radian
+kRho = 0.3
+kAlpha = 0.8
+kBeta = -0.35
+epoch = 0
+thT = $odoth-thTw
+rho = 0
+wl = 0
+wr = 0
+v = 0
+w = 0
+%log "thT" "rho" "wl" "wr" "v" "w"
+%invtrans(xTw,yTw,thTw,$odox,$odoy,$odoth)
+%%%%%%%%%%%%
+log "$odox" "$odoy" "$odoth"
+label "loop"
+epoch = epoch+1
+%%%%%%%%%%%%
+xT = cos(thTw)*($odox-xTw)+sin(thTw)*($odoy-yTw)
+yT = -sin(thTw)*($odox-xTw)+cos(thTw)*($odoy-yTw)
+thT = $odoth-thTw
+thT = normalizeanglerad(thT)
+dx = -xT
+dy = -yT
+rho = sqrt(dx*dx+dy*dy)
+e = atan2(dy, dx)
+alpha = -thT+atan2(dy, dx)
+beta = -thT-alpha
+alpha = normalizeanglerad(alpha)
+alpha = alpha
+beta = normalizeanglerad(beta)
+beta = beta
+%%%%%%%%%%%%%%%%
+v = kRho*rho
+w = kAlpha*alpha+kBeta*beta
+l = 0.26 % measured
+wl = (2*v-w*l)/2
+wr = (2*v+w*l)/2
+motorcmds wl wr
+if (v<0.005) "stop"
+if (epoch>9999) "stop"
+if(rho>0.01|abs($odoth-thTw)>0.02) "loop"
+label "stop"
+stop
+
diff --git a/formerSrc/feedBackControl-cl b/formerSrc/feedBackControl-cl
new file mode 100644
index 0000000000000000000000000000000000000000..6c2b1f024d3ce5a3aeb11689782e9483adc6b392
--- /dev/null
+++ b/formerSrc/feedBackControl-cl
@@ -0,0 +1,42 @@
+%% feedback control
+xTw = -0.5
+yTw = -0.5
+thTW = -180/180*3.1415926535897932384626
+kRho = 0.3
+kAlpha = 0.8
+kBeta = -0.15
+epoch = 0
+invtrans(xTw,yTw,thTw,$odox,$odoy,$odoth)
+
+log "$odox,$odoy,$odoth"
+label "loop"
+epoch = epoch+1
+
+xT = cos(thTw)*($odox-xTw)+sin(thTw)*($odoy-yTw)
+yT = -sin(thTw)*($odox-xTw)+cos(thTw)*($odoy-yTw)
+thT = $odoth-thTw
+dx = -xT
+dy = -yT
+rho = sqrt(dx*dx+dy*dy)
+alpha = -thT+atan2(dy,dx)
+beta = -thT-alpha
+alpha = normalizeanglerad(alpha)
+beta = normalizeanglerad(beta)
+
+v = kRho*rho
+if(v<0.05) "velocity"
+label "continu"
+w = kAlpha*alpha+kBeta*beta
+l = 0.26 %measured
+wr = (2*v-w*l)/2
+wl = (2*v+w*l)/2
+motorcmds wl wr
+if(rho>0.01) "loop"
+
+if(abs(thT)>0.02) "loop"
+if(epoch==10000) "stop"
+label "stop"
+stop
+label "velocity"
+v = v
+goto "continu"
diff --git a/formerSrc/feedBackControl-r b/formerSrc/feedBackControl-r
new file mode 100644
index 0000000000000000000000000000000000000000..fb87bf7e534a09a8090264f83adf61b9c88823f2
--- /dev/null
+++ b/formerSrc/feedBackControl-r
@@ -0,0 +1,63 @@
+
+% feedback control
+
+%% x,y and theta value for goal pose % given in question 8
+xTw = 0
+yTw = 0
+thTw = 90*3.1415926535897932384626/180
+
+% define in moveToTaget function in MATLAB
+% feedback parameters given in q7
+k_rho = 0.3
+k_alpha = 0.8
+k_beta = -0.35
+epoch = 0
+
+% coordinate transformation given in q10
+invtrans(xTw,yTw,thTw,$odox,$odoy,$odoth)
+
+% storing log values
+log "$odox" "$odoy" "$odoth"
+label "loop"
+epoch = epoch+1
+
+%
+xT = cos(thTw)*($odox-xTw)+sin(thTw)*($odoy-yTw)
+yT = -sin(thTw)*($odox-xTw)+cos(thTw)*($odoy-yTw)
+thT = $odoth-thTw
+thT = normalizeanglerad(thT)
+dx = -xT
+dy = -yT
+
+%define in moveTOTarget function in MATLAB
+
+rho = sqrt(dx*dx+dy*dy)
+alpha = -thT+atan2(dy, dx)
+beta = -thT-alpha
+alpha = normalizeanglerad(alpha)
+alpha = alpha%/3.1415926535897932384626*180
+beta = normalizeanglerad(beta)
+beta = beta%/3.1415926535897932384626*180
+
+
+v = k_rho*rho
+
+if(v<0.005) "velocity"
+label "continue"
+%define in moveToTaget function in MATLAB
+w = k_alpha*alpha+k_beta*beta
+% defined in robot2wheel function in MATLAB
+l = 0.26 % measured
+wl = (2*v-w*l)/2 % vl is speed of left wheel
+wr = (2*v+w*l)/2 % vr is speed of right wheel
+motorcmds wl wr % given in the q10
+if(rho>0.01) "loop"
+
+if(abs(thT)>0.02) "loop"
+if (epoch==10000) "stop"
+label "stop"
+stop
+label "velocity"
+v = v
+goto "continue"
+
diff --git a/formerSrc/guidemarks.cl b/formerSrc/guidemarks.cl
new file mode 100644
index 0000000000000000000000000000000000000000..61646f5ec3f303808fa4f70a8e674f36c2d0650b
--- /dev/null
+++ b/formerSrc/guidemarks.cl
@@ -0,0 +1,49 @@
+array "gmx" 15
+array "gmy" 15
+array "gmth" 15
+pi=3.1416
+gmx[1]=0
+gmx[2]=0
+gmx[3]=4
+gmx[4]=4
+gmx[5]=0
+gmx[6]=0
+gmx[7]=4
+gmx[8]=4
+gmx[9]=0
+gmx[10]=4
+gmx[11]=0.4
+gmx[12]=3.6
+gmx[13]=2
+gmx[14]=2
+
+gmy[1]=1.5
+gmy[2]=1.5
+gmy[3]=1.5
+gmy[4]=1.5
+gmy[5]=3.5
+gmy[6]=3.5
+gmy[7]=3.5
+gmy[8]=3.5
+gmy[9]=4.7
+gmy[10]=4.7
+gmy[11]=5
+gmy[12]=5
+gmy[13]=4.0
+gmy[14]=4.0
+
+gmth[1]=pi
+gmth[2]=0
+gmth[3]=pi
+gmth[4]=0
+gmth[5]=pi
+gmth[6]=0
+gmth[7]=pi
+gmth[8]=0
+gmth[9]=0
+gmth[10]=pi
+gmth[11]=-pi/2
+gmth[12]=-pi/2
+gmth[13]=pi
+gmth[14]=0
+
diff --git a/formerSrc/laserl b/formerSrc/laserl
new file mode 100644
index 0000000000000000000000000000000000000000..b6e7ee475e929b5a9ad225d505e7ce26cfeee0c8
--- /dev/null
+++ b/formerSrc/laserl
@@ -0,0 +1,4 @@
+drive @v 0.3 :($irdistfrontleft<0.45)
+turn -90
+stop
+
diff --git a/formerSrc/log b/formerSrc/log
new file mode 100644
index 0000000000000000000000000000000000000000..c4721b00c6dac7e99488eb303c63f7fa4fbc8c97
--- /dev/null
+++ b/formerSrc/log
@@ -0,0 +1,279 @@
+0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
+0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
+0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
+0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
+0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
+0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
+-0.029553 0.000051 -0.195227 0.195257 0.000015 1.501861
+-0.029553 0.000051 -0.195227 0.195257 0.000015 1.501861
+-0.089447 0.000051 -0.201456 0.201486 0.000015 1.549776
+-0.089447 0.000051 -0.201456 0.201486 0.000015 1.549776
+-0.105208 0.000051 -0.203095 0.203125 0.000015 1.562386
+-0.105208 0.000051 -0.203095 0.203125 0.000015 1.562386
+-0.074473 0.000051 -0.199898 0.199929 0.000015 1.537798
+-0.074473 0.000051 -0.199898 0.199929 0.000015 1.537798
+-0.043738 0.000051 -0.196702 0.196733 0.000015 1.513210
+-0.043738 0.000051 -0.196702 0.196733 0.000015 1.513210
+-0.012609 0.000102 -0.193108 0.193170 0.000031 1.485686
+-0.012609 0.000102 -0.193108 0.193170 0.000031 1.485686
+0.018126 0.000102 -0.190861 0.190922 0.000031 1.468397
+0.018126 0.000102 -0.190861 0.190922 0.000031 1.468397
+0.048467 0.000051 -0.183321 0.183352 0.000015 1.410280
+0.048467 0.000051 -0.183321 0.183352 0.000015 1.410280
+0.077626 0.000051 -0.180289 0.180319 0.000015 1.386953
+0.077626 0.000051 -0.180289 0.180319 0.000015 1.386953
+0.105996 0.000051 -0.177338 0.177369 0.000015 1.364257
+0.105996 0.000051 -0.177338 0.177369 0.000015 1.364257
+0.133579 0.000051 -0.174469 0.174500 0.000015 1.342191
+0.133579 0.000051 -0.174469 0.174500 0.000015 1.342191
+0.161162 0.000051 -0.169935 0.169966 0.000015 1.307313
+0.161162 0.000051 -0.169935 0.169966 0.000015 1.307313
+0.187956 0.000051 -0.167149 0.167179 0.000015 1.285877
+0.187956 0.000051 -0.167149 0.167179 0.000015 1.285877
+0.213963 0.000051 -0.164444 0.164475 0.000015 1.265072
+0.213963 0.000051 -0.164444 0.164475 0.000015 1.265072
+0.240757 0.000051 -0.161657 0.161688 0.000015 1.243637
+0.240757 0.000051 -0.161657 0.161688 0.000015 1.243637
+0.265976 0.000051 -0.159035 0.159065 0.000015 1.223462
+0.265976 0.000051 -0.159035 0.159065 0.000015 1.223462
+0.290406 0.000051 -0.156494 0.156525 0.000015 1.203917
+0.290406 0.000051 -0.156494 0.156525 0.000015 1.203917
+0.314837 0.000051 -0.153953 0.153984 0.000015 1.184373
+0.314837 0.000051 -0.153953 0.153984 0.000015 1.184373
+0.339661 0.000101 -0.175549 0.175609 0.000030 1.350606
+0.339661 0.000101 -0.175549 0.175609 0.000030 1.350606
+0.363697 0.000052 -0.147493 0.147524 0.000015 1.134680
+0.363697 0.000052 -0.147493 0.147524 0.000015 1.134680
+0.388916 0.000052 -0.144870 0.144901 0.000015 1.114506
+0.388916 0.000052 -0.144870 0.144901 0.000015 1.114506
+0.414134 0.000052 -0.142248 0.142279 0.000015 1.094331
+0.414134 0.000052 -0.142248 0.142279 0.000015 1.094331
+0.436595 0.000100 -0.170724 0.170784 0.000030 1.313492
+0.436595 0.000100 -0.170724 0.170784 0.000030 1.313492
+0.458267 0.000052 -0.136524 0.136556 0.000016 1.050308
+0.458267 0.000052 -0.136524 0.136556 0.000016 1.050308
+0.481909 0.000052 -0.132844 0.132876 0.000016 1.022000
+0.481909 0.000052 -0.132844 0.132876 0.000016 1.022000
+0.505551 0.000052 -0.130386 0.130417 0.000016 1.003087
+0.505551 0.000052 -0.130386 0.130417 0.000016 1.003087
+0.526829 0.000052 -0.128173 0.128204 0.000016 0.986064
+0.526829 0.000052 -0.128173 0.128204 0.000016 0.986064
+0.546531 0.000052 -0.126124 0.126155 0.000016 0.970303
+0.546531 0.000052 -0.126124 0.126155 0.000016 0.970303
+0.566627 0.000098 -0.163800 0.163859 0.000029 1.260229
+0.566627 0.000098 -0.163800 0.163859 0.000029 1.260229
+0.586723 0.000053 -0.121004 0.121035 0.000016 0.930920
+0.586723 0.000053 -0.121004 0.121035 0.000016 0.930920
+0.608789 0.000053 -0.118709 0.118740 0.000016 0.913267
+0.608789 0.000053 -0.118709 0.118740 0.000016 0.913267
+0.630462 0.000097 -0.160471 0.160529 0.000029 1.234616
+0.630462 0.000097 -0.160471 0.160529 0.000029 1.234616
+0.648981 0.000053 -0.113719 0.113751 0.000016 0.874882
+0.648981 0.000053 -0.113719 0.113751 0.000016 0.874882
+0.670259 0.000053 -0.111506 0.111538 0.000016 0.857859
+0.670259 0.000053 -0.111506 0.111538 0.000016 0.857859
+0.691537 0.000053 -0.109293 0.109325 0.000016 0.840837
+0.691537 0.000053 -0.109293 0.109325 0.000016 0.840837
+0.708087 0.000053 -0.107572 0.107604 0.000016 0.827597
+0.708087 0.000053 -0.107572 0.107604 0.000016 0.827597
+0.725425 0.000053 -0.105769 0.105800 0.000016 0.813727
+0.725425 0.000053 -0.105769 0.105800 0.000016 0.813727
+0.741974 0.000053 -0.104048 0.104079 0.000016 0.800487
+0.741974 0.000053 -0.104048 0.104079 0.000016 0.800487
+0.758918 0.000095 -0.154372 0.154428 0.000028 1.187692
+0.758918 0.000095 -0.154372 0.154428 0.000028 1.187692
+0.775074 0.000053 -0.099965 0.099997 0.000016 0.769082
+0.775074 0.000053 -0.099965 0.099997 0.000016 0.769082
+0.793987 0.000053 -0.097998 0.098030 0.000016 0.753951
+0.793987 0.000053 -0.097998 0.098030 0.000016 0.753951
+0.813689 0.000053 -0.095949 0.095981 0.000016 0.738189
+0.813689 0.000053 -0.095949 0.095981 0.000016 0.738189
+0.828663 0.000053 -0.094391 0.094423 0.000016 0.726210
+0.828663 0.000053 -0.094391 0.094423 0.000016 0.726210
+0.844424 0.000053 -0.092752 0.092784 0.000016 0.713601
+0.844424 0.000053 -0.092752 0.092784 0.000016 0.713601
+0.858610 0.000053 -0.091277 0.091309 0.000016 0.702253
+0.858610 0.000053 -0.091277 0.091309 0.000016 0.702253
+0.872795 0.000053 -0.089802 0.089834 0.000016 0.690904
+0.872795 0.000053 -0.089802 0.089834 0.000016 0.690904
+0.886587 0.000092 -0.148341 0.148396 0.000028 1.141297
+0.886587 0.000092 -0.148341 0.148396 0.000028 1.141297
+0.900378 0.000053 -0.086502 0.086534 0.000016 0.665520
+0.900378 0.000053 -0.086502 0.086534 0.000016 0.665520
+0.918898 0.000091 -0.146376 0.146431 0.000027 1.126181
+0.918898 0.000091 -0.146376 0.146431 0.000027 1.126181
+0.937417 0.000054 -0.081961 0.081993 0.000016 0.630593
+0.937417 0.000054 -0.081961 0.081993 0.000016 0.630593
+0.955543 0.000054 -0.080076 0.080108 0.000016 0.616092
+0.955543 0.000054 -0.080076 0.080108 0.000016 0.616092
+0.973669 0.000054 -0.078191 0.078223 0.000016 0.601592
+0.973669 0.000054 -0.078191 0.078223 0.000016 0.601592
+0.985490 0.000054 -0.076961 0.076994 0.000016 0.592135
+0.985490 0.000054 -0.076961 0.076994 0.000016 0.592135
+0.998099 0.000054 -0.075650 0.075682 0.000016 0.582047
+0.998099 0.000054 -0.075650 0.075682 0.000016 0.582047
+1.010709 0.000054 -0.074339 0.074371 0.000016 0.571960
+1.010709 0.000054 -0.074339 0.074371 0.000016 0.571960
+1.022530 0.000054 -0.072818 0.072850 0.000016 0.560262
+1.022530 0.000054 -0.072818 0.072850 0.000016 0.560262
+1.034351 0.000054 -0.071588 0.071621 0.000016 0.550805
+1.034351 0.000054 -0.071588 0.071621 0.000016 0.550805
+1.045384 0.000054 -0.070441 0.070473 0.000016 0.541978
+1.045384 0.000054 -0.070441 0.070473 0.000016 0.541978
+1.055629 0.000054 -0.069375 0.069408 0.000016 0.533782
+1.055629 0.000054 -0.069375 0.069408 0.000016 0.533782
+1.066662 0.000054 -0.068228 0.068261 0.000016 0.524956
+1.066662 0.000054 -0.068228 0.068261 0.000016 0.524956
+1.077695 0.000054 -0.067081 0.067113 0.000016 0.516129
+1.077695 0.000054 -0.067081 0.067113 0.000016 0.516129
+1.087940 0.000054 -0.066015 0.066048 0.000016 0.507933
+1.087940 0.000054 -0.066015 0.066048 0.000016 0.507933
+1.098973 0.000054 -0.064868 0.064900 0.000016 0.499107
+1.098973 0.000054 -0.064868 0.064900 0.000016 0.499107
+1.110006 0.000054 -0.063720 0.063753 0.000016 0.490280
+1.110006 0.000054 -0.063720 0.063753 0.000016 0.490280
+1.119463 0.000054 -0.062737 0.062769 0.000016 0.482715
+1.119463 0.000054 -0.062737 0.062769 0.000016 0.482715
+1.128920 0.000054 -0.061753 0.061786 0.000016 0.475149
+1.128920 0.000054 -0.061753 0.061786 0.000016 0.475149
+1.138377 0.000054 -0.060770 0.060802 0.000016 0.467584
+1.138377 0.000054 -0.060770 0.060802 0.000016 0.467584
+1.147046 0.000054 -0.059868 0.059901 0.000016 0.460649
+1.147046 0.000054 -0.059868 0.059901 0.000016 0.460649
+1.156503 0.000054 -0.058885 0.058917 0.000016 0.453083
+1.156503 0.000054 -0.058885 0.058917 0.000016 0.453083
+1.165960 0.000054 -0.057901 0.057934 0.000016 0.445518
+1.165960 0.000054 -0.057901 0.057934 0.000016 0.445518
+1.175417 0.000054 -0.056917 0.056950 0.000016 0.437952
+1.175417 0.000054 -0.056917 0.056950 0.000016 0.437952
+1.184086 0.000054 -0.056016 0.056048 0.000016 0.431017
+1.184086 0.000054 -0.056016 0.056048 0.000016 0.431017
+1.193542 0.000054 -0.055032 0.055065 0.000016 0.423451
+1.193542 0.000054 -0.055032 0.055065 0.000016 0.423451
+1.202999 0.000054 -0.054049 0.054081 0.000016 0.415886
+1.202999 0.000054 -0.054049 0.054081 0.000016 0.415886
+1.211668 0.000054 -0.053147 0.053180 0.000016 0.408951
+1.211668 0.000054 -0.053147 0.053180 0.000016 0.408951
+1.220337 0.000054 -0.052246 0.052278 0.000016 0.402016
+1.220337 0.000054 -0.052246 0.052278 0.000016 0.402016
+1.228218 0.000054 -0.051426 0.051459 0.000016 0.395711
+1.228218 0.000054 -0.051426 0.051459 0.000016 0.395711
+1.236099 0.000054 -0.050607 0.050639 0.000016 0.389406
+1.236099 0.000054 -0.050607 0.050639 0.000016 0.389406
+1.243191 0.000054 -0.049869 0.049901 0.000016 0.383732
+1.243191 0.000054 -0.049869 0.049901 0.000016 0.383732
+1.251072 0.000054 -0.049049 0.049082 0.000016 0.377428
+1.251072 0.000054 -0.049049 0.049082 0.000016 0.377428
+1.258953 0.000054 -0.048230 0.048262 0.000016 0.371123
+1.258953 0.000054 -0.048230 0.048262 0.000016 0.371123
+1.266834 0.000054 -0.047410 0.047443 0.000016 0.364818
+1.266834 0.000054 -0.047410 0.047443 0.000016 0.364818
+1.274320 0.000081 -0.129646 0.129695 0.000024 0.997465
+1.274320 0.000081 -0.129646 0.129695 0.000024 0.997465
+1.281807 0.000054 -0.045780 0.045813 0.000016 0.352283
+1.281807 0.000054 -0.045780 0.045813 0.000016 0.352283
+1.295993 0.000054 -0.044305 0.044338 0.000016 0.340935
+1.295993 0.000054 -0.044305 0.044338 0.000016 0.340935
+1.309390 0.000055 -0.042799 0.042832 0.000016 0.329351
+1.309390 0.000055 -0.042799 0.042832 0.000016 0.329351
+1.316483 0.000055 -0.042062 0.042094 0.000016 0.323677
+1.316483 0.000055 -0.042062 0.042094 0.000016 0.323677
+1.322787 0.000055 -0.041406 0.041439 0.000016 0.318633
+1.322787 0.000055 -0.041406 0.041439 0.000016 0.318633
+1.329092 0.000055 -0.040750 0.040783 0.000016 0.313590
+1.329092 0.000055 -0.040750 0.040783 0.000016 0.313590
+1.335396 0.000055 -0.040095 0.040127 0.000016 0.308546
+1.335396 0.000055 -0.040095 0.040127 0.000016 0.308546
+1.340913 0.000055 -0.039486 0.039518 0.000016 0.303861
+1.340913 0.000055 -0.039486 0.039518 0.000016 0.303861
+1.347218 0.000055 -0.038830 0.038863 0.000016 0.298818
+1.347218 0.000055 -0.038830 0.038863 0.000016 0.298818
+1.353522 0.000055 -0.038174 0.038207 0.000016 0.293774
+1.353522 0.000055 -0.038174 0.038207 0.000016 0.293774
+1.359827 0.000055 -0.037518 0.037551 0.000016 0.288730
+1.359827 0.000055 -0.037518 0.037551 0.000016 0.288730
+1.366131 0.000055 -0.036863 0.036896 0.000016 0.283687
+1.366131 0.000055 -0.036863 0.036896 0.000016 0.283687
+1.371648 0.000055 -0.036264 0.036297 0.000017 0.279078
+1.371648 0.000055 -0.036264 0.036297 0.000017 0.279078
+1.377953 0.000055 -0.035608 0.035641 0.000017 0.274035
+1.377953 0.000055 -0.035608 0.035641 0.000017 0.274035
+1.384257 0.000055 -0.034952 0.034985 0.000017 0.268991
+1.384257 0.000055 -0.034952 0.034985 0.000017 0.268991
+1.390562 0.000055 -0.034297 0.034330 0.000017 0.263947
+1.390562 0.000055 -0.034297 0.034330 0.000017 0.263947
+1.396078 0.000055 -0.033723 0.033756 0.000017 0.259534
+1.396078 0.000055 -0.033723 0.033756 0.000017 0.259534
+1.400807 0.000055 -0.033231 0.033264 0.000017 0.255751
+1.400807 0.000055 -0.033231 0.033264 0.000017 0.255751
+1.404747 0.000055 -0.032811 0.032844 0.000017 0.252519
+1.404747 0.000055 -0.032811 0.032844 0.000017 0.252519
+1.409476 0.000055 -0.032319 0.032352 0.000017 0.248736
+1.409476 0.000055 -0.032319 0.032352 0.000017 0.248736
+1.414204 0.000055 -0.031827 0.031860 0.000017 0.244953
+1.414204 0.000055 -0.031827 0.031860 0.000017 0.244953
+1.418933 0.000055 -0.031336 0.031369 0.000017 0.241171
+1.418933 0.000055 -0.031336 0.031369 0.000017 0.241171
+1.423661 0.000055 -0.030844 0.030877 0.000017 0.237388
+1.423661 0.000055 -0.030844 0.030877 0.000017 0.237388
+1.428389 0.000055 -0.030352 0.030385 0.000017 0.233605
+1.428389 0.000055 -0.030352 0.030385 0.000017 0.233605
+1.433118 0.000055 -0.029860 0.029893 0.000017 0.229822
+1.433118 0.000055 -0.029860 0.029893 0.000017 0.229822
+1.437058 0.000055 -0.029447 0.029481 0.000017 0.226646
+1.437058 0.000055 -0.029447 0.029481 0.000017 0.226646
+1.441787 0.000055 -0.028956 0.028989 0.000017 0.222863
+1.441787 0.000055 -0.028956 0.028989 0.000017 0.222863
+1.446515 0.000055 -0.028464 0.028497 0.000017 0.219081
+1.446515 0.000055 -0.028464 0.028497 0.000017 0.219081
+1.451244 0.000055 -0.027972 0.028005 0.000017 0.215298
+1.451244 0.000055 -0.027972 0.028005 0.000017 0.215298
+1.455972 0.000055 -0.027480 0.027514 0.000017 0.211515
+1.455972 0.000055 -0.027480 0.027514 0.000017 0.211515
+1.460701 0.000055 -0.026989 0.027022 0.000017 0.207732
+1.460701 0.000055 -0.026989 0.027022 0.000017 0.207732
+1.465429 0.000055 -0.026497 0.026530 0.000017 0.203950
+1.465429 0.000055 -0.026497 0.026530 0.000017 0.203950
+1.469369 0.000055 -0.026091 0.026124 0.000017 0.200830
+1.469369 0.000055 -0.026091 0.026124 0.000017 0.200830
+1.474098 0.000055 -0.025599 0.025633 0.000017 0.197047
+1.474098 0.000055 -0.025599 0.025633 0.000017 0.197047
+1.478826 0.000055 -0.025108 0.025141 0.000017 0.193264
+1.478826 0.000055 -0.025108 0.025141 0.000017 0.193264
+1.483555 0.000055 -0.024616 0.024649 0.000017 0.189481
+1.483555 0.000055 -0.024616 0.024649 0.000017 0.189481
+1.488283 0.000055 -0.024124 0.024157 0.000017 0.185699
+1.488283 0.000055 -0.024124 0.024157 0.000017 0.185699
+1.492224 0.000055 -0.023714 0.023748 0.000017 0.182546
+1.492224 0.000055 -0.023714 0.023748 0.000017 0.182546
+1.495376 0.000055 -0.023387 0.023420 0.000017 0.180024
+1.495376 0.000055 -0.023387 0.023420 0.000017 0.180024
+1.498134 0.000074 -0.118078 0.118122 0.000022 0.908462
+1.498134 0.000074 -0.118078 0.118122 0.000022 0.908462
+1.500893 0.000055 -0.022819 0.022853 0.000017 0.175662
+1.500893 0.000055 -0.022819 0.022853 0.000017 0.175662
+1.511926 0.000055 -0.021672 0.021705 0.000017 0.166836
+1.511926 0.000055 -0.021672 0.021705 0.000017 0.166836
+1.522959 0.000055 -0.020525 0.020558 0.000017 0.158009
+1.522959 0.000055 -0.020525 0.020558 0.000017 0.158009
+1.526111 0.000055 -0.020197 0.020230 0.000017 0.155488
+1.526111 0.000055 -0.020197 0.020230 0.000017 0.155488
+1.529263 0.000055 -0.019869 0.019902 0.000017 0.152966
+1.529263 0.000055 -0.019869 0.019902 0.000017 0.152966
+1.531628 0.000055 -0.019634 0.019667 0.000017 0.151159
+1.531628 0.000055 -0.019634 0.019667 0.000017 0.151159
+1.534780 0.000055 -0.019306 0.019339 0.000017 0.148637
+1.534780 0.000055 -0.019306 0.019339 0.000017 0.148637
+1.537932 0.000055 -0.018978 0.019012 0.000017 0.146115
+1.537932 0.000055 -0.018978 0.019012 0.000017 0.146115
+1.541084 0.000055 -0.018650 0.018684 0.000017 0.143593
+1.541084 0.000055 -0.018650 0.018684 0.000017 0.143593
+1.544237 0.000055 -0.018323 0.018356 0.000017 0.141072
+1.544237 0.000055 -0.018323 0.018356 0.000017 0.141072
+1.547389 0.000055 -0.017995 0.018028 0.000017 0.138550
+1.547389 0.000055 -0.017995 0.018028 0.000017 0.138550
+1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
+1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
+1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
+1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
+1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
diff --git a/formerSrc/point2point b/formerSrc/point2point
new file mode 100644
index 0000000000000000000000000000000000000000..fab42fe5d2d8687397a6e3fc486b2d4d0714b871
--- /dev/null
+++ b/formerSrc/point2point
@@ -0,0 +1,16 @@
+%we need input (dx,dy,dt)
+dx=-0.5
+dy=-0.5
+dt=-180
+log "$odox" "$odoy" "$odoth"
+arg=atan2(dy, dx)
+%arg = normalizeangledeg(arg)
+wait 0.5
+dis=sqrt(dx*dx+dy*dy)
+arg = arg/3.1415926535932384626*180
+turn arg
+fwd dis
+stop
+arg = -arg
+turn arg
+turn dt
diff --git a/formerSrc/point2point-cl b/formerSrc/point2point-cl
new file mode 100644
index 0000000000000000000000000000000000000000..571d1398835c8abcea1a4a6b20f8b8f2bcfa096f
--- /dev/null
+++ b/formerSrc/point2point-cl
@@ -0,0 +1,14 @@
+dx=0.5
+dy=0
+dtheta=0
+log "$odox" "$odoy" "$odoth"
+angle = atan2(dy,dx)
+wait 0.5
+dis = sqrt(dx*dx+dy*dy)
+angle = agngle/3.1415926535932384626*180
+turn angle
+fwd dis
+stop
+angle = -angle
+turn angle
+turn dtheta
diff --git a/formerSrc/point2point-r b/formerSrc/point2point-r
new file mode 100644
index 0000000000000000000000000000000000000000..48b69297dd6d3c0cf49876662d4c314d3f9e3e2a
--- /dev/null
+++ b/formerSrc/point2point-r
@@ -0,0 +1,20 @@
+%q11 same as q8
+dx=0.5
+dy=-0.5
+dtheta=-90 % orientation
+
+% logging the odometry
+log "$odox" "$odoy" "$odoth"
+
+ang = atan2(dy, dx) % calculating the angle
+ang = ang/3.1415926535932384626*180 % converting angle to radian
+wait 3
+
+dist=sqrt(dx*dx+dy*dy) % distance
+
+turn ang
+fwd dist
+stop
+ang = -ang
+turn ang
+turn dtheta
diff --git a/formerSrc/point2point.save b/formerSrc/point2point.save
new file mode 100644
index 0000000000000000000000000000000000000000..41c65f937b2a49b395a1b9478e82152d3d5e8dc9
--- /dev/null
+++ b/formerSrc/point2point.save
@@ -0,0 +1,16 @@
+x%we need input (dx,dy,dt)
+dx=0.5
+dy=0.5
+dt=0
+log "$odox" "$odoy" "$odoth"
+arg=atan2(dy, dx)
+%arg = normalizeangledeg(arg)
+wait 0.5
+dis=sqrt(dx*dx+dy*dy)
+arg = arg/3.1415926535932384626*180
+turn arg
+fwd dis
+stop
+arg = -arg
+turn arg
+turn dt
diff --git a/formerSrc/qrcode b/formerSrc/qrcode
new file mode 100644
index 0000000000000000000000000000000000000000..0ca69ef85816f54ce6ee02ba8df9b82372f0fd54
--- /dev/null
+++ b/formerSrc/qrcode
@@ -0,0 +1,8 @@
+get "guidemark" :($guidemarkok>1)
+wait 5
+switch($guidemarkok)
+turn 10
+case 2
+fwd 0.5
+stop
+endswitch
diff --git a/formerSrc/ruchir b/formerSrc/ruchir
new file mode 100644
index 0000000000000000000000000000000000000000..7153dadb78f986b4ea79aa2153366d52b8187d57
--- /dev/null
+++ b/formerSrc/ruchir
@@ -0,0 +1,9 @@
+laser “scanpush cmd='zoneobst' “
+turn 30
+log "$I0" "I1" "I2" "I3" "I4" "I5" "I6" "I7" "I8"
+wait 0.5
+turn 30
+wait 0.5
+turn -10
+wait 0.5
+
diff --git a/formerSrc/stop1 b/formerSrc/stop1
new file mode 100644
index 0000000000000000000000000000000000000000..1e13d54dc116a39589fe17fa039ea518e5527455
--- /dev/null
+++ b/formerSrc/stop1
@@ -0,0 +1 @@
+stop
diff --git a/formerSrc/test0 b/formerSrc/test0
new file mode 100644
index 0000000000000000000000000000000000000000..2f5ea9fff57345517a5c737a1f6684bac83dac13
--- /dev/null
+++ b/formerSrc/test0
@@ -0,0 +1,13 @@
+%followline "br" @v 0.2 :($drivendist>2.0)
+%stop
+
+%followline "bl" @v 0.2 :($drivendist>2.0)
+%stop
+
+%followline "bl" @v 0.2 :($crossingblackline)
+%stop
+
+drive @v 0.3:($irdistfrontleft<0.4)|($irdistfrontmiddle<0.4)|($irdistfrontright<0.4)
+stop
+
+
diff --git a/formerSrc/test1 b/formerSrc/test1
new file mode 100644
index 0000000000000000000000000000000000000000..9dbed3378c1d794038385e6dad0708356f56a648
--- /dev/null
+++ b/formerSrc/test1
@@ -0,0 +1,4 @@
+fwd 1.5 @v 0.3
+turn 180
+fwd 1.5 @v 0.3
+stop
diff --git a/formerSrc/test1-cl b/formerSrc/test1-cl
new file mode 100644
index 0000000000000000000000000000000000000000..6fc96ff2f29947f0737a5f35d913057a37859ece
--- /dev/null
+++ b/formerSrc/test1-cl
@@ -0,0 +1,4 @@
+fwd 1.5 @v 0.3
+turn 180
+fwd 1.5 @v 0.3
+turn 180
diff --git a/formerSrc/test10 b/formerSrc/test10
new file mode 100644
index 0000000000000000000000000000000000000000..8cb6c0406e13e5f47abcb9739ffd0a9a10155220
--- /dev/null
+++ b/formerSrc/test10
@@ -0,0 +1,80 @@
+drive @v 0.2 :($irdistfrontleft<0.45)|($irdistfrontmiddle<0.45)|($irdistfrontright<0.45)
+stop
+wait 3
+switch ($fiducialid)
+stop
+case 1 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 2 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+%%%%%%%%%%
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 3 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+%%%%%%%%%%
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 4 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+%%%%%%%%%%
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+endswitch
+
diff --git a/formerSrc/test10-cl b/formerSrc/test10-cl
new file mode 100644
index 0000000000000000000000000000000000000000..2e35d293e0fbc97de841b7e06238e13289b4c186
--- /dev/null
+++ b/formerSrc/test10-cl
@@ -0,0 +1,31 @@
+drive @v 0.2 :($irdistfrontleft<0.45)|($irdistfrontmiddle<0.45)|($irdistfrontright<0.45)
+stop
+wait 3
+switch ($fiducialid)
+case 1
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2s
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn 90 @v 0.2
+drive @v 0.2 :($drivendist > 0.1)
+stop
+followline "bl" @v 0.1
+case 2
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+drive @v 0.2 :($drivendist > 0.1)
+stop
+endswitch
+
diff --git a/formerSrc/test11 b/formerSrc/test11
new file mode 100644
index 0000000000000000000000000000000000000000..bd3e70d3e0ca60f919e84f9840fa98030c0ddbc1
--- /dev/null
+++ b/formerSrc/test11
@@ -0,0 +1,80 @@
+drive @v 0.2 :($irdistfrontmiddle<0.3)
+stop
+get "guidemark" :($guidemarkok>1)
+wait 3
+switch ($guidemarkok)
+stop
+case 1 %%%%%
+turn 180 @v 0.2
+stop
+fwd 0.5
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 2 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+%%%%%%%%%%
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 3 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+%%%%%%%%%%
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 4 %%%%%
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline==1)
+stop
+%%%%%%%%%%
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+endswitch
+
diff --git a/formerSrc/test11-cl b/formerSrc/test11-cl
new file mode 100644
index 0000000000000000000000000000000000000000..e513852b6f82b0560005f98850744b49a5614044
--- /dev/null
+++ b/formerSrc/test11-cl
@@ -0,0 +1,70 @@
+drive @v 0.2 :($irdistfrontleft<0.45)|($irdistfrontmiddle<0.45)|($irdistfrontright<0.45)
+stop
+wait 3
+switch ($fiducialid)
+case 1
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+drive @v 0.2 :($drivendist > 0.1)
+stop
+case 2
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.1 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+drive @v 0.2 :($drivendist > 0.1)
+stop
+case 3
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.1 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.1 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+drive @v 0.2 :($drivendist > 0.1)
+stop
+case 4
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.1 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.1 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.1 @v 0.2
+followline "bm" @v 0.2 :($crossingblackline == 1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+drive @v 0.2 :($drivendist > 0.1)
+stop
diff --git a/formerSrc/test11-r b/formerSrc/test11-r
new file mode 100644
index 0000000000000000000000000000000000000000..605b5a02c0d59acac7031c8cc3064e776ec9aa77
--- /dev/null
+++ b/formerSrc/test11-r
@@ -0,0 +1,88 @@
+% Park according to the number on the guided mark
+
+drive @v 0.2 :($irdistfrontmiddle<0.45) % 0.45
+stop
+% get guided mark seen by the camera. Note camserver should run at this time
+get "guidemark" :($guidemarkok>1)
+wait 5
+
+if the guide mark is found the command will return the code
+in the $guidemarkok variable otherwise it will be less than 1
+
+% Switch case
+switch ($guidemarkok)
+
+% move fwd 0.5
+case 1
+turn 180 @v 0.2 % After reading code turn it back
+stop
+fwd 0.5 % move it 0.5
+turn -180 @v 0.2 % turn it again to face it towards front
+followline "bl" @v 0.3 :($crossingblackline==1) % follow left line
+stop
+fwd 0.25 @v 0.3
+stop
+turn -90 @v 0.3
+drive @v 0.2 :($drivendist>0.1)
+stop
+
+% move fwd 1, bm and bl
+case 2
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.3 :($crossingblackline==1)
+stop
+
+fwd 0.3 @v 0.2
+followline "bm" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.3
+stop
+turn -90 @v 0.3
+drive @v 0.3 :($drivendist>0.1)
+stop
+
+case 3
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.3
+followline "bm" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.3
+followline "br" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+drive @v 0.3 :($drivendist>0.1)
+stop
+
+case 4
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.2
+followline "bm" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.3 @v 0.3
+followline "bm" @v 0.3 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.3
+stop
+turn -90 @v 0.2
+drive @v 0.3 :($drivendist>0.1)
+stop
+endswitch
+
diff --git a/formerSrc/test2 b/formerSrc/test2
new file mode 100644
index 0000000000000000000000000000000000000000..2d5e814bc33434231b2300fb84c2065fa0e10c1e
--- /dev/null
+++ b/formerSrc/test2
@@ -0,0 +1,5 @@
+fwd 1.5 @v 0.3
+turn 180 @v 100
+fwd 1.5 @v 0.3
+turn 180 @v 100
+stop
diff --git a/formerSrc/test2-cl b/formerSrc/test2-cl
new file mode 100644
index 0000000000000000000000000000000000000000..042230207d5aa86f27058eef1c5335fac6aea662
--- /dev/null
+++ b/formerSrc/test2-cl
@@ -0,0 +1,8 @@
+fwd 1
+turn 90
+fwd 0.3
+turn 90
+fwd 1
+turn 90
+fwd 0.3
+turn 90
diff --git a/formerSrc/test3 b/formerSrc/test3
new file mode 100644
index 0000000000000000000000000000000000000000..51f138c05d2e59904dbd692c1833ff1226557b23
--- /dev/null
+++ b/formerSrc/test3
@@ -0,0 +1,9 @@
+fwd 1
+turn 90
+fwd 0.3
+turn 90
+fwd 1
+turn 90
+fwd 0.3
+turn 90
+
diff --git a/formerSrc/test3-cl b/formerSrc/test3-cl
new file mode 100644
index 0000000000000000000000000000000000000000..1bf069d7419eeba49090376c78ab7d48d283052f
--- /dev/null
+++ b/formerSrc/test3-cl
@@ -0,0 +1,14 @@
+drive @v 0.3 :($drivendist > 0.75)
+stop
+drive @v 0.3 :($drivendist > 0.3)
+stop
+turnr 0.3 90 @v 0.3
+drive @v 0.3 :($drivendist > 0.3)
+stop
+turnr 0.3 90 @v 0.3
+drive @v 0.3 :($drivendist > 0.3)
+stop
+turnr 0.3 90 @v 0.3
+drive @v 0.3 :($drivendist > 0.3)
+stop
+turnr 0.3 90 @v 0.3
diff --git a/formerSrc/test3_3.5.2 b/formerSrc/test3_3.5.2
new file mode 100644
index 0000000000000000000000000000000000000000..c04dc6ad40615889730e780adaa262ada5e2521a
--- /dev/null
+++ b/formerSrc/test3_3.5.2
@@ -0,0 +1,10 @@
+drive :($drivendist>1)
+turnr 0.2 90
+drive :($drivendist>0.3)
+turnr 0.2 90
+drive :($drivendist>1)
+turnr 0.2 90
+drive :($drivendist>0.3)
+turnr 0.2 90
+stop
+
diff --git a/formerSrc/test4 b/formerSrc/test4
new file mode 100644
index 0000000000000000000000000000000000000000..cd62c2fed9ea55f5a93e05a73143d4135e2c6b79
--- /dev/null
+++ b/formerSrc/test4
@@ -0,0 +1,19 @@
+drive @v 0.3 :($drivendist>0.75)
+stop
+drive @v 0.3 :($drivendist>0.3)
+stop
+wait 0.5
+turnr 0.3 90 @v 0.1
+drive @v 0.3 :($drivendist>0.3)
+stop
+wait 0.5
+turnr 0.3 90 @v 0.1
+drive @v 0.3 :($drivendist>0.3)
+stop
+wait 0.5
+turnr 0.3 90 @v 0.1
+drive @v 0.3 :($drivendist>0.3)
+stop
+wait 0.5
+turnr 0.3 90 @v 0.1
+
diff --git a/formerSrc/test4-cl b/formerSrc/test4-cl
new file mode 100644
index 0000000000000000000000000000000000000000..4299e164096f851de151ab6ba1c46415768afb5b
--- /dev/null
+++ b/formerSrc/test4-cl
@@ -0,0 +1,2 @@
+followline "bm" @v 0.3 :($drivendist > 1.0)
+stop
diff --git a/formerSrc/test5 b/formerSrc/test5
new file mode 100644
index 0000000000000000000000000000000000000000..d43277c5d941dedc52194d15c7341690db87bbca
--- /dev/null
+++ b/formerSrc/test5
@@ -0,0 +1,3 @@
+followline "bm" @v 0.3 :($drivendist<1)
+fwd 1
+stop
diff --git a/formerSrc/test5-cl b/formerSrc/test5-cl
new file mode 100644
index 0000000000000000000000000000000000000000..a6358efaee0cce4cf508172b912cb828a16d465a
--- /dev/null
+++ b/formerSrc/test5-cl
@@ -0,0 +1,2 @@
+followline "br" @v 0.2 :($drivendist > 2)
+stop
diff --git a/formerSrc/test5_3.5.3 b/formerSrc/test5_3.5.3
new file mode 100644
index 0000000000000000000000000000000000000000..3a56ce3f8148a7e456cb18f346cfb013f45819d7
--- /dev/null
+++ b/formerSrc/test5_3.5.3
@@ -0,0 +1,8 @@
+followline "bm" @v 0.3 :($drivendist<1)
+fwd 1
+stop
+followline "bl" @v 0.002 :($drivendist<1)
+fwd 0.2
+stop
+
+
diff --git a/formerSrc/test6 b/formerSrc/test6
new file mode 100644
index 0000000000000000000000000000000000000000..9089543b9e641b29155cf6de27745d55633b1f7b
--- /dev/null
+++ b/formerSrc/test6
@@ -0,0 +1,3 @@
+followline "bl" @v 0.1 :($drivendist>2)
+stop
+
diff --git a/formerSrc/test6-cl b/formerSrc/test6-cl
new file mode 100644
index 0000000000000000000000000000000000000000..d4de6ba85a612f4665bed1e727aedd5db79a6b54
--- /dev/null
+++ b/formerSrc/test6-cl
@@ -0,0 +1,2 @@
+followline "bl" @v 0.2 :($drivendist > 2)
+stop
diff --git a/formerSrc/test7 b/formerSrc/test7
new file mode 100644
index 0000000000000000000000000000000000000000..b1bbe65fe80bb58edc4d5b4a7cc16127b9bb98bf
--- /dev/null
+++ b/formerSrc/test7
@@ -0,0 +1,3 @@
+followline "bw" @v 0.2 :($crossingblackline==1)
+stop
+
diff --git a/formerSrc/test7-cl b/formerSrc/test7-cl
new file mode 100644
index 0000000000000000000000000000000000000000..b0ff91b71f41848a8efd753537e670cdfe98982a
--- /dev/null
+++ b/formerSrc/test7-cl
@@ -0,0 +1 @@
+followline "bm" @v 0.2 :($crossingblackline==1)
diff --git a/formerSrc/test8-cl b/formerSrc/test8-cl
new file mode 100644
index 0000000000000000000000000000000000000000..5cc6a9a44f01ba6c6b847b3650462f109dfda83d
--- /dev/null
+++ b/formerSrc/test8-cl
@@ -0,0 +1,2 @@
+drive @v 0.2 :($irdistfrontleft<0.4)|($irdistfrontmiddle<0.4)|($irdistfrontright<0.4)
+stop
diff --git a/formerSrc/test8-r b/formerSrc/test8-r
new file mode 100644
index 0000000000000000000000000000000000000000..5cc6a9a44f01ba6c6b847b3650462f109dfda83d
--- /dev/null
+++ b/formerSrc/test8-r
@@ -0,0 +1,2 @@
+drive @v 0.2 :($irdistfrontleft<0.4)|($irdistfrontmiddle<0.4)|($irdistfrontright<0.4)
+stop
diff --git a/formerSrc/test9 b/formerSrc/test9
new file mode 100644
index 0000000000000000000000000000000000000000..ccaa0280b67a15e92e1421bd682facb6414f88dd
--- /dev/null
+++ b/formerSrc/test9
@@ -0,0 +1,33 @@
+drive @v 0.2 :($irdistfrontleft<0.45)|($irdistfrontmiddle<0.45)|($irdistfrontright<0.45)
+stop
+wait 3
+switch ($fiducialid)
+turn 180 @v 0.2
+case 1
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn 90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+case 2
+turn 180 @v 0.2
+stop
+fwd 1
+turn -180 @v 0.2
+followline "bl" @v 0.2 :($crossingblackline==1)
+stop
+fwd 0.25 @v 0.2
+stop
+turn -90 @v 0.2
+%followline "bm" @v 0.1 :($drivendist>0.1)
+drive @v 0.2 :($drivendist>0.1)
+stop
+endswitch
+
diff --git a/formerSrc/text8 b/formerSrc/text8
new file mode 100644
index 0000000000000000000000000000000000000000..3a5f4d5c5114e7e68e6fc38cff34612e35ed2256
--- /dev/null
+++ b/formerSrc/text8
@@ -0,0 +1,2 @@
+drive @v 0.3 :($irdistfrontleft<0.4)|($irdistfrontmiddle<0.4)|($irdistfrontright<0.4)
+stop
diff --git a/formerSrc/try1.png b/formerSrc/try1.png
new file mode 100644
index 0000000000000000000000000000000000000000..d3a6be4556e5c5e513c59c6b43b075ee4ece8249
Binary files /dev/null and b/formerSrc/try1.png differ
diff --git a/formerSrc/try1_zbst b/formerSrc/try1_zbst
new file mode 100644
index 0000000000000000000000000000000000000000..e45a1b29e98b2736e375c4edfdabb0e980baed61
--- /dev/null
+++ b/formerSrc/try1_zbst
@@ -0,0 +1,22 @@
+laser "scanpush cmd ='zone0bst'"
+wait 2
+
+log "$l0" "l0"
+wait 1
+log "$l1" "l1"
+wait 1
+log "$l2" "l2"
+wait 1
+log "$l3" "l3"
+wait 1
+log "$l4" "l4"
+wait 1
+log "$l5" "l5"
+wait 1
+log "$l6" "l6"
+wait 1
+log "$l7" "l7"
+wait 1
+"$l8" "l8"
+wait 1
+%stop
diff --git a/formerSrc/ulmsserver.ini b/formerSrc/ulmsserver.ini
new file mode 100644
index 0000000000000000000000000000000000000000..975c070928aa6e699f0de10777412e20a4b74d4f
--- /dev/null
+++ b/formerSrc/ulmsserver.ini
@@ -0,0 +1,43 @@
+
+server imagepath="./"
+server datapath="./"
+server replayPath="./log"
+
+#Setup server for port 20100+N where N is team nr.
+server port="24919"
+
+#Load basic modules
+module load="odoPose"
+module load="laserPool"
+module load="v360"
+# module load for odometry control and global varable access
+module load=var
+module load=mappose
+
+# live laser scanner on SMR
+#scanset devtype=urg devname="/dev/ttyACM0"
+#scanset def=urg
+#scanset mirror=true
+
+##Simulated scanner (port: 19100+N where N is team nr)
+scanset devType=sim devName=localhost:19001
+scanset def=sim
+
+#Set scanner position with respect of SMR center
+scanset x=0.255 z=0.04
+scanset width=180
+scanset mirror="true"
+scanset logOpen
+scanset log=used
+
+####################################################
+## Load modules and enter setup commands below ##
+####################################################
+module load="aupoly.so.0"
+module load="aulocalize.so.0"
+addline startx=0.0 starty=0.0 endx=1.8 endy=0.0 name="bottom"
+addline startx=1.8 starty=0.0 endx=1.8 endy=1.8 name="right"
+addline startx=0.0 starty=0.0 endx=0.0 endy=1.8 name="left"
+addline startx=0.0 starty=1.8 endx=1.8 endy=1.8 name="top"
+setinitpose x=0.90 y=0.90 th=0
+setinitcov Cx=0.04 Cy=0.04 Cth=0.04
\ No newline at end of file
diff --git a/formerSrc/ulmsserver13.ini b/formerSrc/ulmsserver13.ini
new file mode 100644
index 0000000000000000000000000000000000000000..ce4714edef853c10cad53816618937acdc838d15
--- /dev/null
+++ b/formerSrc/ulmsserver13.ini
@@ -0,0 +1,91 @@
+
+server imagepath="./"
+server datapath="./"
+server replayPath="./log"
+
+#Setup server for port 20100+N where N is team nr.
+server port="24919"
+
+#Load basic modules
+module load="odoPose"
+module load="laserPool"
+module load="v360"
+# module load for odometry control and global varable access
+module load=var
+module load=mappose
+
+# live laser scanner on SMR
+#scanset devtype=urg devname="/dev/ttyACM0"
+#scanset def=urg
+#scanset mirror=true
+
+##Simulated scanner (port: 19100+N where N is team nr)
+scanset devType=sim devName=localhost:19001
+scanset def=sim
+
+#Set scanner position with respect of SMR center
+scanset x=0.255 z=0.04
+scanset width=180
+scanset mirror="true"
+scanset logOpen
+scanset log=used
+
+####################################################
+## Load modules and enter setup commands below ##
+####################################################
+module load="aupoly.so.0"
+module load="aulocalize.so.0"
+module load="auplan.so.0"
+%module load="auzoneobst.so.0"
+setInitPose x=1.7 y=0.3 th=0
+setInitCov Cx=0.04 Cy=0.04 Cth=0.04
+resetplanner
+addpoint pno=1 x=0.2 y=0.3
+addpoint pno=2 x=1.7 y=0.3
+addpoint pno=3 x=3.2 y=0.3
+addpoint pno=4 x=0.2 y=1.05
+addpoint pno=5 x=0.95 y=1.05
+addpoint pno=6 x=1.7 y=1.05
+addpoint pno=7 x=3.2 y=1.05
+addpoint pno=8 x=0.2 y=1.8
+addpoint pno=9 x=0.95 y=1.8
+addpoint pno=10 x=2.45 y=1.8
+addpoint pno=11 x=0.2 y=2.55
+addpoint pno=12 x=1.7 y=2.55
+addpoint pno=13 x=0.2 y=3.3
+addpoint pno=14 x=1.7 y=3.3
+addpoint pno=15 x=2.45 y=3.3
+addpoint pno=16 x=3.2 y=3.3
+addcon pno1=1 pno2=2
+addcon pno1=2 pno2=1
+addcon pno1=2 pno2=3
+addcon pno1=3 pno2=2
+addcon pno1=1 pno2=4
+addcon pno1=4 pno2=1
+addcon pno1=2 pno2=6
+addcon pno1=6 pno2=2
+addcon pno1=4 pno2=5
+addcon pno1=5 pno2=4
+addcon pno1=6 pno2=7
+addcon pno1=7 pno2=6
+addcon pno1=5 pno2=9
+addcon pno1=9 pno2=5
+addcon pno1=7 pno2=16
+addcon pno1=16 pno2=7
+addcon pno1=8 pno2=9
+addcon pno1=9 pno2=8
+addcon pno1=9 pno2=10
+addcon pno1=10 pno2=9
+addcon pno1=8 pno2=11
+addcon pno1=11 pno2=8
+addcon pno1=10 pno2=15
+addcon pno1=15 pno2=10
+addcon pno1=11 pno2=12
+addcon pno1=12 pno2=11
+addcon pno1=12 pno2=14
+addcon pno1=14 pno2=12
+addcon pno1=13 pno2=14
+addcon pno1=14 pno2=13
+addcon pno1=15 pno2=16
+addcon pno1=16 pno2=15
+calculatecost
\ No newline at end of file
diff --git a/guidemarks.cl b/guidemarks.cl
new file mode 100644
index 0000000000000000000000000000000000000000..61646f5ec3f303808fa4f70a8e674f36c2d0650b
--- /dev/null
+++ b/guidemarks.cl
@@ -0,0 +1,49 @@
+array "gmx" 15
+array "gmy" 15
+array "gmth" 15
+pi=3.1416
+gmx[1]=0
+gmx[2]=0
+gmx[3]=4
+gmx[4]=4
+gmx[5]=0
+gmx[6]=0
+gmx[7]=4
+gmx[8]=4
+gmx[9]=0
+gmx[10]=4
+gmx[11]=0.4
+gmx[12]=3.6
+gmx[13]=2
+gmx[14]=2
+
+gmy[1]=1.5
+gmy[2]=1.5
+gmy[3]=1.5
+gmy[4]=1.5
+gmy[5]=3.5
+gmy[6]=3.5
+gmy[7]=3.5
+gmy[8]=3.5
+gmy[9]=4.7
+gmy[10]=4.7
+gmy[11]=5
+gmy[12]=5
+gmy[13]=4.0
+gmy[14]=4.0
+
+gmth[1]=pi
+gmth[2]=0
+gmth[3]=pi
+gmth[4]=0
+gmth[5]=pi
+gmth[6]=0
+gmth[7]=pi
+gmth[8]=0
+gmth[9]=0
+gmth[10]=pi
+gmth[11]=-pi/2
+gmth[12]=-pi/2
+gmth[13]=pi
+gmth[14]=0
+
diff --git a/initrd.img b/initrd.img
new file mode 100644
index 0000000000000000000000000000000000000000..beba6736a26a3db9fbeb812df3731f7d9ee64c70
Binary files /dev/null and b/initrd.img differ
diff --git a/initrd.img.old b/initrd.img.old
new file mode 100644
index 0000000000000000000000000000000000000000..ee58bde2626cbaf436d368c2c6054672e002b5af
Binary files /dev/null and b/initrd.img.old differ
diff --git a/laser_0.log b/laser_0.log
new file mode 100644
index 0000000000000000000000000000000000000000..ffeddfa85a468a38293288c4c23445e0e53eba91
--- /dev/null
+++ b/laser_0.log
@@ -0,0 +1 @@
+11.690000 1167 -0.5 90 361 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 3.731 3.445 3.200 2.987 2.802 2.679 2.683 2.686 2.690 2.695 2.699 2.704 2.709 2.714 2.719 2.725 2.731 2.737 2.743 2.750 2.756 2.763 2.771 2.778 2.786 2.794 2.802 2.811 2.820 2.829 2.838 2.848 2.858 2.868 2.878 2.889 2.900 2.869 2.815 2.764 2.714 2.666 2.621 2.577 2.534 2.493 2.454 2.416 2.380 3.771 3.791 0.001 0.001 0.001 0.001 0.001 0.001 3.160 3.120 3.082 3.045 3.009 2.974 2.940 2.907 2.875 2.844 2.814 2.784 2.756 2.728 3.538 3.565 3.594 3.623 3.653 3.683 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 3.738 3.725 3.713 3.702 3.691 3.680 3.669 3.659 3.649 3.639 3.630 3.621 3.613 3.604 3.596 3.589 3.581 3.574 3.567 3.561 3.555 3.549 3.543 3.538 3.533 3.528 3.524 3.520 3.516 3.512 3.509 3.506 3.503 3.500 3.498 3.496 3.494 3.493 3.492 3.491 3.490 3.490 3.490 3.490 3.490 3.491 3.492 3.493 3.494 3.496 3.498 3.500 3.503 3.506 3.509 3.512 3.516 3.447 3.233 3.044 2.876 2.726 2.591 2.469 2.358 2.257 2.164 2.079 2.000 1.927 1.860 1.797 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 1.348 1.315 1.284 1.255 1.227 1.201 1.175 1.151 1.128 1.106 1.085 1.064 1.045 1.026 1.008 0.991 0.974 0.958 0.943 0.928 0.913 0.900 0.886 0.873 0.861 0.849 0.837 0.826 0.815 0.804 0.794 0.784 0.774 0.765 0.756 0.747 0.739 0.730 0.722 0.715 0.707 0.700 0.692 0.685 0.679 0.672 0.666 0.659 0.653 0.647 0.642 0.636 0.630 0.625 0.620 0.615 0.610 0.605 0.600 0.596 0.591 0.587 0.583 0.579 0.575 0.571 0.567 0.563 0.559 0.556 0.552 0.549 0.546 0.542 0.539 0.536 0.533 0.530 0.527 0.524 0.522 0.519 0.517 0.514 0.512 0.509 0.507 0.505 0.502 0.500 0.498 0.496 0.494 0.492 0.490 0.488 0.487 0.485 0.483 0.482 0.480 0.478 0.477 0.475 0.474 0.473 0.471 0.470 0.469 0.468 0.466 0.465 0.464 0.463 0.462 0.461 0.460 0.460 0.459 0.458 0.457 0.456 0.456 0.455 0.454 0.454 0.453 0.453 0.452 0.452 0.452 0.451 0.451 0.451 0.450 0.450 0.450 0.450 0.450 0.450 0.450 0.450 0 1 0
diff --git a/log b/log
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/log_auclient/.PATH.TEST b/log_auclient/.PATH.TEST
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/log_qclient/.PATH.TEST b/log_qclient/.PATH.TEST
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/log_ucamserver/.PATH.TEST b/log_ucamserver/.PATH.TEST
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/log_ulmsserver/.PATH.TEST b/log_ulmsserver/.PATH.TEST
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/main.sh b/main.sh
new file mode 100644
index 0000000000000000000000000000000000000000..40a355e1e9a161194c0215f88d510c387ad41417
--- /dev/null
+++ b/main.sh
@@ -0,0 +1,42 @@
+#!usr/bin/bash
+# terminal 1: laser server
+gnome-terminal -t "laser server" -x bash -c "cd /home/smr/sim/bash_src/;
+sh laser.sh;exec bash;"
+sleep 1
+# terminal 2: run the simulator
+gnome-terminal -t "simulator" -x bash -c "cd /home/smr/sim/bash_src/;
+sh sim.sh;exec bash;"
+sleep 1
+# terminal 3: run the client
+gnome-terminal -t "client" -x bash -c "cd /home/smr/sim/bash_src/;
+sh client.sh;exec bash;"
+sleep 1
+# terminal 4: run mrc scripts
+gnome-terminal -t "mrc" -x bash -c "cd /home/smr/sim/bash_src/;
+sh mov.sh;exec bash;"
+sleep 1
+# terminate
+## print processes
+echo "processes"
+ps -ef | grep "bash" | grep -v grep | awk '{print $2}'
+ps
+while true;do
+echo "Enter 'e' to exit"
+read flag
+echo $flag
+ if [ ! -n "$flag" ]
+ then
+ echo "Invalid input"
+ else
+ if [ $flag=='e' ]
+ then
+ ## clean logg files
+ rm -r *.logg
+ ## kill all terminal processes
+ gnome-terminal -t "clear all" -x killall -9 bash
+ break
+ fi
+ fi
+done
+exit 0
+
diff --git a/maze b/maze
new file mode 100644
index 0000000000000000000000000000000000000000..c33abaeabb912d8fca69bbd4667e2a7a0cf1ced8
--- /dev/null
+++ b/maze
@@ -0,0 +1,6 @@
+-0.25 -0.14 5.25 -0.14
+5.25 -0.14 5.25 5.64
+5.25 5.64 -0.25 5.64
+-0.25 5.64 -0.25 -0.14
+-0.25 3.64 3.25 3.64
+1.75 2.14 5.25 2.14
diff --git a/mrc_src/createWalls b/mrc_src/createWalls
new file mode 100644
index 0000000000000000000000000000000000000000..77f1fa0b71f3a50fa9842eaa7592dd5980319487
--- /dev/null
+++ b/mrc_src/createWalls
@@ -0,0 +1,21 @@
+%% group 16
+%% 31388 Advanced Autonomous Robots
+
+% draw the maze in ulmsserver
+laser "addline startx=0 starty=0 endx=1.8 endy=0 name='bottom left'"
+laser "addline startx=2.2 starty=0 endx=4.0 endy=0 name='bottom right'"
+laser "addline startx=0 starty=5.0 endx=1.8 endy=0 name='top left'"
+laser "addline startx=2.2 starty=5.0 endx=4.0 endy=5.0 name='top right'"
+laser "addline startx=0 starty=0 endx=0 endy=1.8 name='left down'"
+laser "addline startx=0 starty=3.2 endx=0 endy=5 name='left up'"
+laser "addline startx=4 starty=0 endx=4 endy=1.8 name='right down'"
+laser "addline startx=4 starty=3.2 endx=4 endy=5 name='right up'"
+laser "addline startx=0.9 starty=3.1 endx=1.7 endy=3.1 name='maze bottom left'"
+laser "addline startx=2.3 starty=3.1 endx=3.1 endy=3.1 name='maze bottom right'"
+laser "addline startx=1.7 starty=2.5 endx=1.7 endy=3.1 name='maze left down'"
+laser "addline startx=0.9 starty=3.1 endx=0.9 endy=4.3 name='maze left up'"
+laser "addline startx=2.3 starty=2.5 endx=2.3 endy=3.1 name='maze right down'"
+laser "addline startx=3.1 starty=3.1 endx=3.1 endy=4.3 name='maze right up'"
+laser "addline startx=1.5 starty=3.7 endx=2.5 endy=3.7 name='maze middle horizontal'"
+laser "addline startx=2.0 starty=3.7 endx=2.0 endy=4.3 name='maze middle vertical'"
+laser "addline startx=0.9 starty=4.3 endx=3.1 endy=4.3 name='maze top'"
\ No newline at end of file
diff --git a/mrc_src/data.debug b/mrc_src/data.debug
new file mode 100644
index 0000000000000000000000000000000000000000..9636c159a9f98ca4e07ec9f7525ed54624fa6e07
--- /dev/null
+++ b/mrc_src/data.debug
@@ -0,0 +1,74 @@
+Debug file for the robot initialization
+---------------------------------------
+Sat May 23 14:32:24 2020
+---------------------------------------
+
+Module : robotinfo
+ type differential enum number 0
+ name smr
+
+Module : odometry
+ cl 0.000102
+ cr 0.000102
+ w 0.260000
+ robotlength 0.265000
+ ts 0.010000
+ maxtick 1000
+ control 0
+ enctype 0
+
+Module : motion control
+ ts 0.010000
+ line_gain 0.050000
+ line_tau 10.000000
+ line_alfa 0.300000
+ wall_gain 1.000000
+ wall_tau 0.700000
+ wall_alfa 0.200000
+ gain 1.000000
+ tau 0.160000
+ alfa 0.200000
+ drive_kangle 2.250000
+ drive_kdist 4.800000
+ w 0.260000
+ robotlength 0.265000
+ lim 0.200000
+ stopdist 0.180000
+ alarmdist 0.100000
+ velcmd 0.300000
+ acccmd 0.300000
+ nolinedist 0.200000
+
+Module : motor control
+ velscalel 100.000000
+ velscaler 100.000000
+ kp 66.000000
+ ki 10.000000
+
+Module : line sensor
+ size 8
+ k_filt 0.750000
+
+Module : filter
+ measurementnoise_std_x 0.100000
+ measurementnoise_std_y 0.110000
+ processnoise_std_stearingangle 0.010000
+ processnoise_std_dist 0.020000
+ type EKF
+ run 1
+ use 0
+ size_input 2
+ size_state 3
+ size_noise_process 2
+ size_output 2
+ size_noise_output 2
+ gpsoffset_easting 0.000000
+ gpsoffset_northing 0.000000
+
+Module : gpsmouse
+ hostname localhost
+ port 9500
+ SBAS 0
+ run 1
+ use 1
+
diff --git a/mrc_src/guidemarks b/mrc_src/guidemarks
new file mode 100644
index 0000000000000000000000000000000000000000..36285e9a8e1b2af59c98453d14e8230369786022
--- /dev/null
+++ b/mrc_src/guidemarks
@@ -0,0 +1,49 @@
+% prior guidemark information
+array "gmx" 15
+array "gmy" 15
+array "gmth" 15
+pi=3.1415926535
+gmx[1]=0
+gmx[2]=0
+gmx[3]=4
+gmx[4]=4
+gmx[5]=0
+gmx[6]=0
+gmx[7]=4
+gmx[8]=4
+gmx[9]=0
+gmx[10]=4
+gmx[11]=0.4
+gmx[12]=3.6
+gmx[13]=2
+gmx[14]=2
+
+gmy[1]=1.5
+gmy[2]=1.5
+gmy[3]=1.5
+gmy[4]=1.5
+gmy[5]=3.5
+gmy[6]=3.5
+gmy[7]=3.5
+gmy[8]=3.5
+gmy[9]=4.7
+gmy[10]=4.7
+gmy[11]=5
+gmy[12]=5
+gmy[13]=4.0
+gmy[14]=4.0
+
+gmth[1]=pi
+gmth[2]=0
+gmth[3]=pi
+gmth[4]=0
+gmth[5]=pi
+gmth[6]=0
+gmth[7]=pi
+gmth[8]=0
+gmth[9]=0
+gmth[10]=pi
+gmth[11]=-pi/2
+gmth[12]=-pi/2
+gmth[13]=pi
+gmth[14]=0
\ No newline at end of file
diff --git a/mrc_src/log b/mrc_src/log
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/mrc_src/main b/mrc_src/main
new file mode 100644
index 0000000000000000000000000000000000000000..ba11398002cb62016aea11a4cf486e43afc009b8
--- /dev/null
+++ b/mrc_src/main
@@ -0,0 +1,234 @@
+%% group 16
+%% 31388 Advanced Autonomous Robots
+
+%%%%%%%%%%%%%%%%CONSTANTS%%%%%%%%%%%%%%%%%%%%%%%
+pi = 3.1415926535
+% origin
+xOrig = 0.25
+yOrig = 0.45
+% distance from the expected robot position to the guidemark
+dist = 0.25
+% distance from the nearest node to the guidemark
+nodeDist = 0.15
+% robot length
+length = 0.35
+% the first guidemark number
+gmno = 11
+% task number
+%% - 1: subtask1, follow the guidemarks
+%% - 2: subtask2, recognize unknown object
+%% - 0: finish the full task, subtask1->subtask2
+task = 1
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% prior guidemark information
+array "gmx" 15
+array "gmy" 15
+array "gmth" 15
+
+% x coordinates
+gmx[1]=0
+gmx[2]=0
+gmx[3]=4
+gmx[4]=4
+gmx[5]=0
+gmx[6]=0
+gmx[7]=4
+gmx[8]=4
+gmx[9]=0
+gmx[10]=4
+gmx[11]=0.4
+gmx[12]=3.6
+gmx[13]=2
+gmx[14]=2
+
+% y coordinates
+gmy[1]=1.5
+gmy[2]=1.5
+gmy[3]=1.5
+gmy[4]=1.5
+gmy[5]=3.5
+gmy[6]=3.5
+gmy[7]=3.5
+gmy[8]=3.5
+gmy[9]=4.7
+gmy[10]=4.7
+gmy[11]=5
+gmy[12]=5
+gmy[13]=4.0
+gmy[14]=4.0
+
+% theta
+gmth[1]=pi
+gmth[2]=0
+gmth[3]=pi
+gmth[4]=0
+gmth[5]=pi
+gmth[6]=0
+gmth[7]=pi
+gmth[8]=0
+gmth[9]=0
+gmth[10]=pi
+gmth[11]=-pi/2
+gmth[12]=-pi/2
+gmth[13]=pi
+gmth[14]=0
+%% get cost for graph planner
+laser "calculatecost"
+
+%% set initial odopose
+set "$odox" 0
+set "$odoy" 0
+set "$odoth" 0
+
+%% Kalman filter localization
+laser "localize"
+laser “push t='0.3' cmd='localize' “
+wait 1
+
+%%%%%%%%MAIN PROCESS%%%%%%%%%%
+%
+switch(task)
+call "subtask1"
+call "subtask2"
+call "backToOrig"
+goto "end"
+case 1
+call "subtask1"
+call "backToOrig"
+goto "end"
+case 2
+call "subtask2"
+call "backToOrig"
+goto "end"
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% function: subtask1
+% follow guidemarks
+label "subtask1"
+if(gmno<1) "backToOrig"
+% set a destination
+% if the guidemark is on the right side of a wall
+if(gmth[gmno]==0) "rightSide"
+% if the guidemark is on the left side of a wall
+if(gmth[gmno]==pi) "leftSide"
+xEnd = gmx[gmno]
+yEnd = gmy[gmno]-0.5
+label "top"
+thEnd = pi+gmth[gmno]
+eval xEnd
+eval yEnd
+eval thEnd
+% go to the next guidemark
+call "plan"
+% read the guidemark, update gmno
+call "look"
+if(gmno!=98) "subtask1"
+return
+label "rightSide"
+xEnd = gmx[gmno]+(nodeDist+length)
+yEnd = gmy[gmno]
+goto "top"
+label "leftSide"
+xEnd = gmx[gmno]-(nodeDist+length)
+yEnd = gmy[gmno]
+goto "top"
+
+% function: subtask2
+% recoggnize unknow objects
+label "subtask2"
+return
+
+% function: update
+% get the robot position in world coordinates
+label "update"
+invtrans $l0 $l1 $l2 $odox $odoy $odoth
+wait 1
+xCurr = $res0
+yCurr = $res1
+thCurr = $res2
+eval xCurr
+eval yCurr
+eval thCurr
+return
+
+% function: go
+% drive the robot to the target pose
+label "go"
+call "update"
+eval xTar
+eval yTar
+eval thTar
+rotAng = atan2(yTar-yCurr,xTar-xCurr)-thCurr
+rotAng = normalizeanglerad(rotAng)
+wait 0.5
+eval rotAng
+ignoreobstacles
+turn rotAng "rad" @v 0.1 :($cmdtime>5)|($irdistfrontmiddle<=0.05)
+wait 0.5
+ignoreobstacles
+drivew xTar yTar thTar "rad" @v 0.5 :($targetdist<0.01)|($cmdtime>10)|($irdistfrontmiddle<=0.05)
+stop
+call "update"
+% correct orientation
+rotAng = thTar-thCurr
+rotAng = normalizeanglerad(rotAng)
+wait 0.5
+ignoreobstacles
+turn rotAng "rad" @v 0.1 :($cmdtime>5)|($irdistfrontmiddle<=0.05)
+wait 0.5
+return
+
+% function: plan
+% drive the robot to a given position in a maze, using graph planner
+label "plan"
+call "update"
+stringcat "findroute startx="xCurr"starty="yCurr"endx="xEnd"endy="yEnd
+laser "$string"
+wait 1
+pno = $l4
+label "loop"
+pno = pno-1
+stringcat "getpoint p="pno
+laser "$string"
+wait 0.5
+xTar = $l5
+yTar = $l6
+thTar = $l7
+% remove the verbose pose
+if((xTar==xOrig)&(yTar==yOrig)) "loop"
+eval pno
+call "go"
+wait 0.5
+if(pno>0) "loop"
+% drive the robot to head at the target
+xTar = xEnd
+yTar = yEnd
+thTar = thEnd
+call "go"
+wait 0.5
+ignoreobstacles
+drive @v 0.1 :($irdistfrontmiddle<=dist)
+wait 0.5
+return
+
+% function: look
+% return guidemarkNumber
+label "look"
+get "guidemark" :($fiducialid>=1)|($cmdtime>30)
+eval $guidemarkok
+eval $fiducialid
+gmno = $guidemarkok
+eval $gmkx
+eval $gmky
+eval $gmkz
+return
+
+% function: backToOrig
+% drive the robot back to the start square
+label "backToOrig"
+return
+
+
+label "end"
+stop
diff --git a/mrc_src/spawnNodes b/mrc_src/spawnNodes
new file mode 100644
index 0000000000000000000000000000000000000000..aa9d1535bdc9d5fd55bb3cc6a1a9dfb544fcac09
--- /dev/null
+++ b/mrc_src/spawnNodes
@@ -0,0 +1,84 @@
+%% group 16
+%% 31388 Advanced Autonomous Robots
+
+% graph planner
+%% spawn nodes on the map and add connections
+%%% add nodes
+laser "addpoint pno=1 x=0.25 y=0.45"
+laser "addpoint pno=2 x=3.5 y=0.45"
+laser "addpoint pno=3 x=-0.5 y=1.5"
+laser "addpoint pno=4 x=0.5 y=1.5"
+laser "addpoint pno=5 x=3.5 y=1.5"
+laser "addpoint pno=6 x=4.5 y=1.5"
+laser "addpoint pno=7 x=-0.5 y=2.5"
+laser "addpoint pno=8 x=0.5 y=2.5"
+laser "addpoint pno=9 x=2 y=2"
+laser "addpoint pno=10 x=3.5 y=2.5"
+laser "addpoint pno=11 x=4.5 y=2.5"
+laser "addpoint pno=12 x=-0.5 y=3.5"
+laser "addpoint pno=13 x=0.5 y=3.5"
+laser "addpoint pno=14 x=1.3 y=3.3"
+laser "addpoint pno=15 x=2 y=3.3"
+laser "addpoint pno=16 x=2.7 y=3.3"
+laser "addpoint pno=17 x=3.5 y=3.5"
+laser "addpoint pno=18 x=4.5 y=3.5"
+laser "addpoint pno=19 x=1.3 y=4"
+laser "addpoint pno=20 x=2.7 y=4"
+laser "addpoint pno=21 x=0.5 y=4.5"
+laser "addpoint pno=22 x=3.5 y=4.5"
+laser "addpoint pno=23 x=1.5 y=4"
+laser "addpoint pno=24 x=2.5 y=4"
+laser "addpoint pno=25 x=2 y=0.5"
+
+%%% add connections
+laser "addcon pno1=1 pno2=25"
+laser "addcon pno1=25 pno2=1"
+laser "addcon pno1=1 pno2=4"
+laser "addcon pno1=2 pno2=25"
+laser "addcon pno1=25 pno2=2"
+laser "addcon pno1=2 pno2=5"
+laser "addcon pno1=3 pno2=7"
+laser "addcon pno1=4 pno2=1"
+laser "addcon pno1=4 pno2=8"
+laser "addcon pno1=5 pno2=2"
+laser "addcon pno1=5 pno2=10"
+laser "addcon pno1=6 pno2=11"
+laser "addcon pno1=7 pno2=3"
+laser "addcon pno1=7 pno2=8"
+laser "addcon pno1=7 pno2=12"
+laser "addcon pno1=8 pno2=4"
+laser "addcon pno1=8 pno2=7"
+laser "addcon pno1=8 pno2=9"
+laser "addcon pno1=8 pno2=13"
+laser "addcon pno1=9 pno2=8"
+laser "addcon pno1=9 pno2=10"
+laser "addcon pno1=9 pno2=15"
+laser "addcon pno1=10 pno2=5"
+laser "addcon pno1=10 pno2=9"
+laser "addcon pno1=10 pno2=11"
+laser "addcon pno1=10 pno2=17"
+laser "addcon pno1=11 pno2=6"
+laser "addcon pno1=11 pno2=18"
+laser "addcon pno1=12 pno2=7"
+laser "addcon pno1=13 pno2=8"
+laser "addcon pno1=13 pno2=21"
+laser "addcon pno1=14 pno2=15"
+laser "addcon pno1=14 pno2=19"
+laser "addcon pno1=15 pno2=9"
+laser "addcon pno1=15 pno2=14"
+laser "addcon pno1=15 pno2=16"
+laser "addcon pno1=16 pno2=15"
+laser "addcon pno1=16 pno2=20"
+laser "addcon pno1=17 pno2=10"
+laser "addcon pno1=17 pno2=22"
+laser "addcon pno1=18 pno2=11"
+laser "addcon pno1=19 pno2=14"
+laser "addcon pno1=20 pno2=16"
+laser "addcon pno1=21 pno2=13"
+laser "addcon pno1=21 pno2=22"
+laser "addcon pno1=22 pno2=17"
+laser "addcon pno1=22 pno2=21"
+laser "addcon pno1=19 pno2=23"
+laser "addcon pno1=20 pno2=24"
+laser "addcon pno1=23 pno2=19"
+laser "addcon pno1=24 pno2=20"
\ No newline at end of file
diff --git a/nodes.vsdx b/nodes.vsdx
new file mode 100644
index 0000000000000000000000000000000000000000..abc0c95b3a85d2b2c2c4e7b48143bda3c12787af
Binary files /dev/null and b/nodes.vsdx differ
diff --git a/object_detect.so.0 b/object_detect.so.0
new file mode 100644
index 0000000000000000000000000000000000000000..11e6d65a069d5a9dffc40d8ed0a7c5f317e67b6b
Binary files /dev/null and b/object_detect.so.0 differ
diff --git a/odoPose.log b/odoPose.log
new file mode 100644
index 0000000000000000000000000000000000000000..b1aa6ba6cab3f372e68d5bf43dfcce9c1a5b9a76
--- /dev/null
+++ b/odoPose.log
@@ -0,0 +1,137 @@
+43.550000 0.250 0.000 0.00000 0.000 -1 1589978726.331746
+43.570000 0.250 0.250 0.00000 0.000 -1 1589978726.371911
+46.990000 0.250 0.250 -0.00550 0.000 -1 1589978729.780203
+47.110000 0.253 0.250 -0.00590 0.000 -1 1589978729.910461
+47.170000 0.256 0.250 -0.00590 0.000 -1 1589978729.972338
+47.220000 0.260 0.250 -0.00630 0.000 -1 1589978730.020815
+47.260000 0.263 0.250 -0.00630 0.000 -1 1589978730.060200
+47.300000 0.267 0.250 -0.00630 0.000 -1 1589978730.100743
+47.330000 0.270 0.250 -0.00590 0.000 -1 1589978730.130003
+47.360000 0.273 0.250 -0.00590 0.000 -1 1589978730.164415
+47.390000 0.277 0.250 -0.00590 0.000 -1 1589978730.190372
+47.420000 0.281 0.250 -0.00590 0.000 -1 1589978730.220418
+47.450000 0.285 0.250 -0.00590 0.000 -1 1589978730.250062
+47.470000 0.288 0.250 -0.00550 0.000 -1 1589978730.270413
+47.490000 0.291 0.250 -0.00550 0.000 -1 1589978730.290309
+47.510000 0.294 0.250 -0.00590 0.000 -1 1589978730.310529
+49.690000 0.296 0.250 0.00040 0.000 -1 1589978732.510604
+49.720000 0.296 0.250 0.00590 0.000 -1 1589978732.541967
+49.740000 0.296 0.250 0.01140 0.000 -1 1589978732.560111
+49.760000 0.296 0.250 0.01770 0.000 -1 1589978732.581050
+49.780000 0.296 0.250 0.02560 0.000 -1 1589978732.600998
+49.800000 0.296 0.250 0.03310 0.000 -1 1589978732.619990
+49.820000 0.296 0.250 0.04220 0.000 -1 1589978732.640722
+49.840000 0.296 0.250 0.05240 0.000 -1 1589978732.660543
+49.850000 0.296 0.250 0.05790 0.000 -1 1589978732.670055
+49.860000 0.296 0.250 0.06340 0.000 -1 1589978732.679948
+49.870000 0.296 0.250 0.06940 0.000 -1 1589978732.690283
+49.880000 0.296 0.250 0.07530 0.000 -1 1589978732.700593
+49.890000 0.296 0.250 0.08160 0.000 -1 1589978732.710374
+49.900000 0.296 0.250 0.08790 0.000 -1 1589978732.720544
+49.910000 0.296 0.250 0.09500 0.000 -1 1589978732.729971
+49.920000 0.296 0.250 0.10170 0.000 -1 1589978732.740944
+49.930000 0.296 0.250 0.10840 0.000 -1 1589978732.750321
+49.940000 0.296 0.250 0.11620 0.000 -1 1589978732.760756
+49.950000 0.296 0.250 0.12410 0.000 -1 1589978732.770159
+49.960000 0.296 0.250 0.13160 0.000 -1 1589978732.780105
+49.970000 0.296 0.250 0.13990 0.000 -1 1589978732.791009
+49.980000 0.296 0.250 0.14860 0.000 -1 1589978732.800019
+49.990000 0.296 0.250 0.15720 0.000 -1 1589978732.810083
+50.000000 0.296 0.250 0.16550 0.000 -1 1589978732.820026
+50.010000 0.296 0.250 0.17460 0.000 -1 1589978732.830716
+50.020000 0.296 0.250 0.18400 0.000 -1 1589978732.840060
+50.030000 0.296 0.250 0.19350 0.000 -1 1589978732.852918
+50.040000 0.296 0.250 0.20290 0.000 -1 1589978732.860503
+50.050000 0.296 0.250 0.21320 0.000 -1 1589978732.870474
+50.060000 0.296 0.250 0.22340 0.000 -1 1589978732.880363
+50.070000 0.296 0.250 0.23370 0.000 -1 1589978732.890930
+50.080000 0.296 0.250 0.24470 0.000 -1 1589978732.900223
+50.090000 0.296 0.250 0.25570 0.000 -1 1589978732.910639
+50.100000 0.296 0.250 0.26600 0.000 -1 1589978732.920532
+50.110000 0.296 0.250 0.27780 0.000 -1 1589978732.964051
+50.120000 0.296 0.250 0.28960 0.000 -1 1589978732.964064
+50.130000 0.296 0.250 0.30070 0.000 -1 1589978732.964074
+50.140000 0.296 0.250 0.31330 0.000 -1 1589978732.964083
+50.150000 0.296 0.250 0.32550 0.000 -1 1589978732.970574
+50.160000 0.296 0.250 0.33770 0.000 -1 1589978732.980082
+50.170000 0.296 0.250 0.35110 0.000 -1 1589978732.990418
+50.180000 0.296 0.250 0.36370 0.000 -1 1589978733.000376
+50.190000 0.296 0.250 0.37710 0.000 -1 1589978733.011853
+50.200000 0.296 0.250 0.39010 0.000 -1 1589978733.021723
+50.210000 0.296 0.250 0.40390 0.000 -1 1589978733.030140
+50.220000 0.296 0.250 0.41810 0.000 -1 1589978733.040137
+50.230000 0.296 0.250 0.43150 0.000 -1 1589978733.050372
+50.240000 0.296 0.250 0.44640 0.000 -1 1589978733.061514
+50.250000 0.296 0.250 0.46060 0.000 -1 1589978733.070090
+50.260000 0.296 0.250 0.47560 0.000 -1 1589978733.080336
+50.270000 0.296 0.250 0.49060 0.000 -1 1589978733.091267
+50.280000 0.296 0.250 0.50630 0.000 -1 1589978733.100715
+50.290000 0.296 0.250 0.52130 0.000 -1 1589978733.110355
+50.300000 0.296 0.250 0.53710 0.000 -1 1589978733.121358
+50.310000 0.296 0.250 0.55280 0.000 -1 1589978733.132833
+50.320000 0.296 0.250 0.56940 0.000 -1 1589978733.140169
+50.330000 0.296 0.250 0.58510 0.000 -1 1589978733.151149
+50.340000 0.296 0.250 0.60250 0.000 -1 1589978733.160497
+50.350000 0.296 0.250 0.61900 0.000 -1 1589978733.170660
+50.360000 0.296 0.250 0.63640 0.000 -1 1589978733.184773
+50.370000 0.296 0.250 0.65370 0.000 -1 1589978733.190067
+50.380000 0.296 0.250 0.67180 0.000 -1 1589978733.202716
+50.390000 0.296 0.250 0.68920 0.000 -1 1589978733.213869
+50.400000 0.296 0.250 0.70730 0.000 -1 1589978733.220441
+50.410000 0.296 0.250 0.72540 0.000 -1 1589978733.232161
+50.420000 0.296 0.250 0.74390 0.000 -1 1589978733.240413
+50.430000 0.296 0.250 0.76250 0.000 -1 1589978733.252754
+50.440000 0.296 0.250 0.78140 0.000 -1 1589978733.260206
+50.450000 0.296 0.250 0.80110 0.000 -1 1589978733.270094
+50.460000 0.296 0.250 0.82000 0.000 -1 1589978733.280541
+50.470000 0.296 0.250 0.84010 0.000 -1 1589978733.290223
+50.480000 0.296 0.250 0.86020 0.000 -1 1589978733.299942
+50.490000 0.296 0.250 0.87990 0.000 -1 1589978733.310267
+50.500000 0.296 0.250 0.90040 0.000 -1 1589978733.320268
+50.510000 0.296 0.250 0.92090 0.000 -1 1589978733.330161
+50.520000 0.296 0.250 0.94140 0.000 -1 1589978733.339989
+50.530000 0.296 0.250 0.96260 0.000 -1 1589978733.349967
+50.540000 0.296 0.250 0.98390 0.000 -1 1589978733.360052
+50.550000 0.296 0.250 1.00520 0.000 -1 1589978733.370453
+50.560000 0.296 0.250 1.02730 0.000 -1 1589978733.380736
+50.570000 0.296 0.250 1.04930 0.000 -1 1589978733.390749
+50.580000 0.296 0.250 1.07140 0.000 -1 1589978733.400820
+50.590000 0.296 0.250 1.09390 0.000 -1 1589978733.410942
+50.600000 0.296 0.250 1.11550 0.000 -1 1589978733.421845
+50.610000 0.296 0.250 1.13680 0.000 -1 1589978733.464246
+50.620000 0.296 0.250 1.15810 0.000 -1 1589978733.464259
+50.630000 0.296 0.250 1.17860 0.000 -1 1589978733.464268
+50.640000 0.296 0.250 1.19830 0.000 -1 1589978733.464276
+50.650000 0.296 0.250 1.21800 0.000 -1 1589978733.471655
+50.660000 0.296 0.250 1.23690 0.000 -1 1589978733.480922
+50.670000 0.296 0.250 1.25540 0.000 -1 1589978733.490753
+50.680000 0.296 0.250 1.27310 0.000 -1 1589978733.500056
+50.690000 0.296 0.250 1.29090 0.000 -1 1589978733.512281
+50.700000 0.296 0.250 1.30780 0.000 -1 1589978733.521164
+50.710000 0.296 0.250 1.32400 0.000 -1 1589978733.530456
+50.720000 0.296 0.250 1.34010 0.000 -1 1589978733.540262
+50.730000 0.296 0.250 1.35550 0.000 -1 1589978733.550013
+50.740000 0.296 0.250 1.37010 0.000 -1 1589978733.559977
+50.750000 0.296 0.250 1.38470 0.000 -1 1589978733.570269
+50.760000 0.296 0.250 1.39840 0.000 -1 1589978733.580666
+50.770000 0.296 0.250 1.41180 0.000 -1 1589978733.591859
+50.780000 0.296 0.250 1.42440 0.000 -1 1589978733.600847
+50.790000 0.296 0.250 1.43710 0.000 -1 1589978733.612070
+50.800000 0.296 0.250 1.44850 0.000 -1 1589978733.620894
+50.810000 0.296 0.250 1.45910 0.000 -1 1589978733.630843
+50.820000 0.296 0.250 1.47020 0.000 -1 1589978733.640130
+50.830000 0.296 0.250 1.48000 0.000 -1 1589978733.650174
+50.840000 0.296 0.250 1.48910 0.000 -1 1589978733.661462
+50.850000 0.296 0.250 1.49850 0.000 -1 1589978733.671000
+50.860000 0.296 0.250 1.50720 0.000 -1 1589978733.681952
+50.870000 0.296 0.250 1.51470 0.000 -1 1589978733.690688
+50.880000 0.296 0.250 1.52140 0.000 -1 1589978733.700653
+50.890000 0.296 0.250 1.52850 0.000 -1 1589978733.710076
+50.900000 0.296 0.250 1.53480 0.000 -1 1589978733.721896
+50.910000 0.296 0.250 1.54030 0.000 -1 1589978733.730262
+50.930000 0.296 0.250 1.54860 0.000 -1 1589978733.753427
+60.940000 0.296 0.250 1.54540 0.000 -1 1589978743.820276
+70.950000 0.296 0.250 1.54540 0.000 -1 1589978753.952553
+80.960000 0.296 0.250 1.54540 0.000 -1 1589978764.350658
+90.970000 0.296 0.250 1.54540 0.000 -1 1589978774.512458
+100.980000 0.296 0.250 1.54540 0.000 -1 1589978784.519991
diff --git a/qclient.cmd b/qclient.cmd
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/qclient.ini b/qclient.ini
new file mode 100644
index 0000000000000000000000000000000000000000..a28ddebc3afae27a9793b57d75a7f346395ba8f8
--- /dev/null
+++ b/qclient.ini
@@ -0,0 +1,42 @@
+# AU Client initiation script
+server imagepath="./"
+server datapath="./"
+server replayPath="./"
+module load=var
+module load=odopose
+module load=utmpose
+module load=mappose
+module load=imagepool
+module load=aupoly.so.0
+# interface to camera server
+module load=if alias=cam
+module load=ucamif.so.0
+camdata add=gmk
+camdata add=path
+camdata add=img
+camdata add=cam
+camOnConnect cmd="cam poolpush img=0 cmd='poolget all'"
+camOnConnect cmd="cam poolpush img=1 cmd='poolget all'"
+cam connect=localhost:24920
+module load=if alias=laser
+module load=aulaserif.so.0
+laser connect=localhost:24919
+laserdata add=obst
+module load=aulaserifscan.so.0
+module load=audisp.so.0
+laserOnConnect cmd='laser varpush struct=road cmd="pass getonly"'
+laserOnConnect cmd='laser varpush struct=road cmd="road"'
+laserOnConnect cmd='laser varpush struct=obst cmd="obst update fixed"'
+laserOnConnect cmd='laser odoposepush cmd="odopose pose"'
+laserOnConnect cmd='laser mapposepush cmd="mappose pose"'
+laserOnConnect cmd='laser utmposepush cmd="utmpose pose"'
+laserOnConnect cmd="laser v360push i=15 cmd='scanget pose'"
+laserOnConnect cmd='laser scanpush cmd="scanget pose"'
+laserOnConnect cmd="laser varpush struct=poly cmd='poly update'"
+laserOnConnect cmd="laser poly get"
+disp scale=6
+disp pos=1.2
+disp robot=smr
+module load=if alias=mrc1
+var mrc1.enableXML=0
+mrc1 connect=localhost:31001
diff --git a/test b/test
new file mode 100644
index 0000000000000000000000000000000000000000..2ff07a07e26783e76dcada4647d43c07f39267b3
--- /dev/null
+++ b/test
@@ -0,0 +1 @@
+turn 90
\ No newline at end of file
diff --git a/ufunczoneobst.cpp b/ufunczoneobst.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29f0e9bb1bfb178b816365259ec1beea18a121dd
--- /dev/null
+++ b/ufunczoneobst.cpp
@@ -0,0 +1,147 @@
+/***************************************************************************
+ * Copyright (C) 2005 by Christian Andersen and DTU *
+ * jca@oersted.dtu.dk *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#include "ufunczoneobst.h"
+
+#ifdef LIBRARY_OPEN_NEEDED
+
+/**
+ * This function is needed by the server to create a version of this plugin */
+UFunctionBase * createFunc()
+{ // create an object of this type
+ /** replace 'UFunczoneobst' with your class name */
+ return new UFunczoneobst();
+}
+#endif
+
+bool UFunczoneobst::setResource(UResBase * resource, bool remove) { // load resource as provided by the server (or other plugins)
+ bool result = true;
+
+ if (resource->isA(UResPoseHist::getOdoPoseID())) { // pointer to server the resource that this plugin can provide too
+ // but as there might be more plugins that can provide the same resource
+ // use the provided
+ if (remove)
+ // the resource is unloaded, so reference must be removed
+ poseHist = NULL;
+ else if (poseHist != (UResPoseHist *) resource)
+ // resource is new or is moved, save the new reference
+ poseHist = (UResPoseHist *) resource;
+ else
+ // reference is not used
+ result = false;
+ }
+
+ // other resource types may be needed by base function.
+ result = UFunctionBase::setResource(resource, remove);
+ return result;
+}
+
+
+
+///////////////////////////////////////////////////
+///////////////////////////////////////////////////
+///////////////////////////////////////////////////
+///////////////////////////////////////////////////
+// #define SMRWIDTH 0.4
+bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
+{ // handle a plugin command
+ const int MRL = 500;
+ char reply[MRL];
+ bool ask4help;
+ const int MVL = 30;
+ char value[MVL];
+ ULaserData * data;
+ double robotwidth;
+ //
+ int i,j,imax;
+ double r,delta;
+ double minRange; // min range in meter
+ // double minAngle = 0.0; // degrees
+// double d,robotwidth;
+ double zone[9];
+ // check for parameters - one parameter is tested for - 'help'
+ ask4help = msg->tag.getAttValue("help", value, MVL);
+ if (ask4help)
+ { // create the reply in XML-like (html - like) format
+ sendHelpStart(msg, "zoneobst");
+ sendText("--- available zoneobst options\n");
+ sendText("help This message\n");
+ sendText("fake=F Fake some data 1=random, 2-4 a fake corridor\n");
+ sendText("device=N Laser device to use (see: SCANGET help)\n");
+ sendText("see also: SCANGET and SCANSET\n");
+ sendHelpDone();
+ }
+ else
+ { // do some action and send a reply
+ data = getScan(msg, (ULaserData*)extra);
+ //
+ if (data->isValid())
+ {
+ // check if a attribute (parameter) with name width exists and if so get its value
+ bool gotwidth = msg->tag.getAttValue("width", value, MVL);
+ if (gotwidth) {
+ robotwidth=strtod(value, NULL);
+ }
+ else {
+ robotwidth=0.26;
+ }
+ UPose poseAtScan = poseHist->getPoseAtTime(data->getScanTime());
+ // Gets the odometry pose at the time when the laserscan was taken, poseAtScan.x poseAtScan.y poseAtScan.a (x,y,angle)
+ // make analysis for closest measurement
+ minRange = 1000; // long range in meters
+ imax=data->getRangeCnt();
+ delta=imax/9.0;
+ for (j=0;j<9;j++)
+ zone[j]=minRange;
+ for(j=0;j<9;j++){
+ for (i = 0+(int)(j*delta); i < (int)((j+1)*delta); i++)
+ { // range are stored as an integer in current units
+ r = data->getRangeMeter(i);
+ if (r >= 0.020)
+ { // less than 20 units is a flag value for URG scanner
+ if (r<zone[j])
+ zone[j]=r;
+ }
+ }
+ }
+ /* SMRCL reply format */
+ //snprintf(reply, MRL, "<laser l10=\"%g\" l11=\"%g\" l12=\"%g\" l13=\"%g\" l14=\"%g\" "
+ // "l15=\"%g\" l16=\"%g\" l17=\"%g\" l18=\"%g\" />\n",
+ snprintf(reply, MRL, "<laser l1=\"%g\" l1=\"%g\" l2=\"%g\" l3=\"%g\" l4=\"%g\" "
+ "l5=\"%g\" l6=\"%g\" l7=\"%g\" l8=\"%g\" />\n",
+ zone[0],zone[1],zone[2],zone[3],zone[4],
+ zone[5],zone[6],zone[7],zone[8]);
+ // send this string as the reply to the client
+ sendMsg(msg, reply);
+ // save also as gloabl variable
+ for(i = 0; i < 9; i++)
+ var_zone->setValued(zone[i], i);
+ }
+ else
+ sendWarning(msg, "No scandata available");
+ }
+ // return true if the function is handled with a positive result
+ // used if scanpush or push has a count of positive results
+ return true;
+}
+
+void UFunczoneobst::createBaseVar()
+{ // add also a global variable (global on laser scanner server) with latest data
+ var_zone = addVarA("zone", "0 0 0 0 0 0 0 0 0", "d", "Value of each laser zone. Updated by zoneobst.");
+}
diff --git a/ufunczoneobst.h b/ufunczoneobst.h
new file mode 100644
index 0000000000000000000000000000000000000000..cbb1e90f511f577b2ef96e4ede52ed07df026a72
--- /dev/null
+++ b/ufunczoneobst.h
@@ -0,0 +1,65 @@
+/***************************************************************************
+ * Copyright (C) 2005 by Christian Andersen and DTU *
+ * jca@oersted.dtu.dk *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, write to the *
+ * Free Software Foundation, Inc., *
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
+ ***************************************************************************/
+#ifndef UFUNC_MRCOBST_H
+#define UFUNC_MRCOBST_H
+
+#include <cstdlib>
+
+#include <ulms4/ufunclaserbase.h>
+#include <urob4/uresposehist.h>
+
+
+////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////
+
+/**
+ * Laserscanner function to demonstrate
+ * simple laser scanner data handling and analysis
+ * @author Christian Andersen
+*/
+class UFunczoneobst : public UFuncLaserBase
+{
+public:
+ /**
+ Constructor */
+ UFunczoneobst()
+ { // set the command (or commands) handled by this plugin
+ setCommand("zoneobst", "zoneobstif", "obstacle detect for MRC (Compiled " __DATE__ " " __TIME__ ")");
+ createBaseVar();
+ }
+ virtual bool setResource(UResBase * resource, bool remove);
+ /**
+ Handle incomming command
+ (intended for command separation)
+ Must return true if the function is handled -
+ otherwise the client will get a failed - reply */
+ virtual bool handleCommand(UServerInMsg * msg, void * extra);
+
+ protected:
+ void createBaseVar();
+ UVariable *var_zone;
+ UResPoseHist * poseHist;
+};
+
+
+
+#endif
+
diff --git a/ulmsserver.ini b/ulmsserver.ini
new file mode 100644
index 0000000000000000000000000000000000000000..bf141699b0a44e0cd7fffbd0c4e5f5f6192d8a6f
--- /dev/null
+++ b/ulmsserver.ini
@@ -0,0 +1,44 @@
+
+server imagepath="./"
+server datapath="./"
+server replayPath="./log"
+
+#Setup server for port 20100+N where N is team nr.
+server port="24919"
+
+#Load basic modules
+module load="odoPose"
+module load="laserPool"
+module load="v360"
+# module load for odometry control and global varable access
+module load=var
+module load=mappose
+
+# live laser scanner on SMR
+#scanset devtype=urg devname="/dev/ttyACM0"
+#scanset def=urg
+#scanset mirror=true
+
+##Simulated scanner (port: 19100+N where N is team nr)
+scanset devType=sim devName=localhost:19001
+scanset def=sim
+
+#Set scanner position with respect of SMR center
+scanset x=0.255 z=0.04
+scanset width=180
+scanset mirror="true"
+scanset logOpen
+scanset log=used
+
+####################################################
+## Load modules and enter setup commands below ##
+####################################################
+## log navigation module
+module load="aupoly.so.0"
+module load="aulocalize.so.0"
+module load='auplan.so.0'
+## initialize graph planner
+resetplanner
+## set Kalman parameters
+setInitPose x=0.25 y=0.45 th=0
+setInitCov Cx=0.01 Cy=0.01 Cth=0.01
\ No newline at end of file