Exercise 11, Problem 4

Cleaning the box.
Make a program that 'cleans' the box by going from side to side and back in a pattern that covers the hold box. Aulocalize should be used to assure correct position in the box.

N=10

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "push t='1.0' cmd='localize'"

turn 45
fwd 0.7
turn 135
drivew -0.5 0.5 -180:($targetdist<0.05)
drivew 0.5 0.45 0:($targetdist<0.05)
drivew -0.5 0.4 -180:($targetdist<0.05)
drivew 0.5 0.35 0:($targetdist<0.05)
drivew -0.5 0.3 -180:($targetdist<0.05)
drivew 0.5 0.25 0:($targetdist<0.05)
drivew -0.5 0.2 -180:($targetdist<0.05)
drivew 0.5 0.15 0:($targetdist<0.05)
drivew -0.5 0.1 -180:($targetdist<0.05)
drivew 0.5 0.05 0:($targetdist<0.05)
drivew -0.5 0.0 -180:($targetdist<0.05)
drivew 0.5 -0.05 0:($targetdist<0.05)
drivew -0.5 -0.1 -180:($targetdist<0.05)
drivew 0.5 -0.15 0:($targetdist<0.05)
drivew -0.5 -0.2 -180:($targetdist<0.05)
drivew 0.5 -0.25 0:($targetdist<0.05)
drivew -0.5 -0.3 -180:($targetdist<0.05)
drivew 0.5 -0.35 0:($targetdist<0.05)
drivew -0.5 -0.4 -180:($targetdist<0.05)
drivew 0.5 -0.45 0:($targetdist<0.05)
drivew -0.5 -0.5 -180:($targetdist<0.05)
drivew 0.5 0.5 90:($targetdist<0.05)

Exercise 11, Problem 3

Boxdriving with Kalman Localization.

Change   the   code   from  problem  two   so   it   uses   kalman   updates   from  the localizer plugin.

periodic Kalman filtering

N=10

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "push t='1.0' cmd='localize'"

label "run"
stop
ignoreobstacles
drivew 0.4 0.4 90:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 0.4 -180:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 -0.4 -90:($targetdist<0.05)
stop
ignoreobstacles
drivew 0.4 -0.4 0:($targetdist<0.05)
N=N-1
if(N>0) "run"

each corner Kalman filtering

N=10

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "localize"

label "run"
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 0.5 90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 0.5 -180:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 -0.5 -90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 -0.5 0:($targetdist<0.05)
N=N-1
if(N>0) "run"

Exercise 11, Problem 2

Generation of Boxdriving in SMRCL

Make smrcl  code  that  makes an smr  follow  the  targetposes starting  in  the middle   of   the   box  then   going   to   the   corners and   then   continueing N times around in the square defined by the cornerpoints.

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "localize"

ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)
ignoreobstacles
drive -0.4 0.4 -180:($targetdist<0.05)
ignoreobstacles
drive -0.4 -0.4 -90:($targetdist<0.05)
ignoreobstacles
drive 0.4 -0.4 0:($targetdist<0.05)
ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)

Exercise 11, Problem 1

 Initialization of the localiser.

laser cmd="addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser cmd="addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser cmd="setinitpose x=0 y=0 th=0"
laser cmd="setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser cmd="localize"

 The localizer is know ready. It will make a new pose estimate each time the command  'localize'  is given.