diff --git a/cpp_src/objectDetect.so.0 b/cpp_src/objectDetect.so.0
new file mode 100644
index 0000000000000000000000000000000000000000..29843454db7783544546cb5b873437a5bf8e7164
Binary files /dev/null and b/cpp_src/objectDetect.so.0 differ
diff --git a/cpp_src/ufunczoneobst.cpp b/cpp_src/ufunczoneobst.cpp
index 8f3eafc13c4c4a6c914add7b9b4cc9d0bb16ddca..b316dd145ef425244f4dfde586a5e891a23ec73b 100644
--- a/cpp_src/ufunczoneobst.cpp
+++ b/cpp_src/ufunczoneobst.cpp
@@ -248,20 +248,20 @@ bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
{
angle1 = calcAngle(0,2);
if(angle1<0)
- {angle1 += 180;}
+ {angle1 += PI;}
angle2 = calcAngle(1,3);
if(angle2<0)
- {angle2 += 180;}
+ {angle2 += PI;}
angle = (angle1+angle2)/2;
}
if(calcDist(vex_x[0], vex_y[0], vex_x[3], vex_y[3]) == width)
{
angle1 = calcAngle(0,3);
if(angle1<0)
- {angle1 += 180;}
+ {angle1 += PI;}
angle2 = calcAngle(1,2);
if(angle2<0)
- {angle2 += 180;}
+ {angle2 += PI;}
angle = (angle1+angle2)/2;
}
cout<<"angle: "<<angle<<endl;
@@ -572,6 +572,6 @@ double UFunczoneobst::calcAngle(int idx1, int idx2)
{
double dx = vex_x[idx2] - vex_x[idx1];
double dy = vex_y[idx2] - vex_y[idx1];
-double angle = atan2(dy,dx)*180/PI;
+double angle = atan2(dy,dx);
return angle;
}
\ No newline at end of file
diff --git a/cpp_src/ufunczoneobst.o b/cpp_src/ufunczoneobst.o
index ca57ef6ad962cef5620ebd4f3410d0c1ae3f1754..fc1452f7b98e653fa84b1271f81d8455065411b1 100644
Binary files a/cpp_src/ufunczoneobst.o and b/cpp_src/ufunczoneobst.o differ
diff --git a/data.debug b/data.debug
deleted file mode 100644
index 31c7752f555349be4240f3685120a2561a7dcff1..0000000000000000000000000000000000000000
--- a/data.debug
+++ /dev/null
@@ -1,74 +0,0 @@
-Debug file for the robot initialization
----------------------------------------
-Sun May 24 22:00:05 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
deleted file mode 100644
index 4e6853c82360c70f888950ba715abdb5dfacbfee..0000000000000000000000000000000000000000
--- a/datalog
+++ /dev/null
@@ -1,12 +0,0 @@
-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/detectResult.log b/detectResult.log
new file mode 100644
index 0000000000000000000000000000000000000000..736d25147b6416eb63a43fdff78e81ebef0cb921
--- /dev/null
+++ b/detectResult.log
@@ -0,0 +1 @@
+shape: Triangle center: [1.63446 ,1.45036] width: 0.315473 height: 0.136582 angle: -172.468
diff --git a/gmk.log b/gmk.log
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/initrd.img b/initrd.img
deleted file mode 100644
index beba6736a26a3db9fbeb812df3731f7d9ee64c70..0000000000000000000000000000000000000000
Binary files a/initrd.img and /dev/null differ
diff --git a/initrd.img.old b/initrd.img.old
deleted file mode 100644
index ee58bde2626cbaf436d368c2c6054672e002b5af..0000000000000000000000000000000000000000
Binary files a/initrd.img.old and /dev/null differ
diff --git a/main.sh b/main.sh
index 191f36e9aa7733a6939d1afa359465a31fd5e386..48fafcf3bd966357e6055ebcfb38d603b0044d64 100644
--- a/main.sh
+++ b/main.sh
@@ -78,6 +78,8 @@ else
rm -f createWalls
mv createWalls_temp createWalls
fi
+ cd ..
+ mv /*.log /logs/.
## kill all terminal processes
gnome-terminal -t "close client" -x killall -9 qclient
gnome-terminal -t "clear all" -x killall -9 bash
diff --git a/mrc_src/data.debug b/mrc_src/data.debug
index b1713bfb320de0a47507c9ab17e571c4f5036e6c..7ef98d2fdb980c252014a301ed8a9c80a44ee0c1 100644
--- a/mrc_src/data.debug
+++ b/mrc_src/data.debug
@@ -1,6 +1,6 @@
Debug file for the robot initialization
---------------------------------------
-Wed May 27 18:58:01 2020
+Sun May 31 15:13:12 2020
---------------------------------------
Module : robotinfo
diff --git a/mrc_src/main b/mrc_src/main
index 20bad8faeb657aef69bd7b9403669f18f2adf2b8..c35d92a517e5e2aa1bc63963efb5bd9e5a28e100 100644
--- a/mrc_src/main
+++ b/mrc_src/main
@@ -14,11 +14,10 @@ length = 0.35
dist = 0.5-0.26
% the first guidemark number
gmno = 13
-% task number
-%% - 1: subtask1, follow the guidemarks
-%% - 2: subtask2, recognize unknown object
-%% - 0: finish the full task, subtask1->subtask2
-task = 2
+% shift value
+xshift = 0
+yshift = 0
+thshift = 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% prior guidemark information
@@ -77,33 +76,30 @@ gmth[14]=0
laser "calculatecost"
%% path for detecting the object
-array "dox" 9
-array "doy" 9
-array "doth" 9
-dox[1] = 0.5
-doy[1] = 1.5
-doth[1] = 0
-dox[2] = 1
-doy[2] = 2.5
-doth[2] = -pi/4
-dox[3] = 1.5
-doy[3] = 4
-doth[3] = 0
-dox[4] = 3
-doy[4] = 2.5
-doth[4] = -3*pi/4
-dox[5] = 3.5
-doy[5] = 1.5
-doth[5] = pi
-dox[6] = 3
-doy[6] = 0.5
-doth[6] = 3*pi/4
-dox[7] = 2
-doy[7] = 0.5
-doth[7] = pi/2
-dox[8] = 1.01
-doy[8] = 0.5
-doth[8] = pi/4
+array "dox" 8
+array "doy" 8
+array "doth" 8
+dox[1] = 3
+doy[1] =2.5
+doth[1] = -3*pi/4
+dox[2] = 3.5
+doy[2] =1.5
+doth[2] = pi
+dox[3] = 3
+doy[3] = 0.5
+doth[3] = 3*pi/4
+dox[4] = 2
+doy[4] = 0.5
+doth[4] = pi/2
+dox[5] = 1.01
+doy[5] = 0.5
+doth[5] = pi/4
+dox[6] = 0.5
+doy[6] = 1.5
+doth[6] = 0
+dox[7] = 1
+doy[7] = 2.5
+doth[7] = -pi/4
%% set initial odopose
@@ -123,21 +119,42 @@ laser "odopose log=true"
%%%%%%%%MAIN PROCESS%%%%%%%%%%
%
-switch(task)
+%% first follow guidemarks
+call "calibrate"
call "subtask1"
+% now the robot is in guidemark 13 or 14
+fwd -0.32 @v 0.1
+wait 1
call "subtask2"
call "backToOrig"
goto "end"
-case 1
-call "subtask1"
-wait 5
-call "backToOrig"
-goto "end"
-case 2
-call "subtask2"
-%call "backToOrig"
-goto "end"
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%function: calibrate
+% calculate x-shift and y-shift value of the laser
+label "calibrate"
+xEnd = 0.5
+yEnd = 1.5
+thEnd = pi/2
+call "plan"
+wait 0.5
+xCurr = $odox+xOrig
+yCurr = $odoy+yOrig
+thCurr = $odoth
+eval xCurr
+eval yCurr
+eval thCurr
+xshift = xEnd-xCurr
+yshift = yEnd-yCurr
+thShift = thEnd-thCurr
+xEnd = xEnd
+yEnd = yEnd
+thEnd = thEnd
+eval xshift
+eval yshift
+eval thshift
+call "plan"
+return
+
% function: subtask1
% follow guidemarks
@@ -174,7 +191,25 @@ goto "top"
% function: subtask2
% recoggnize unknow objects
label "subtask2"
-dist = 0.48-0.26
+xEnd = 2
+yEnd = 3.3
+thEnd = -pi/2
+call "plan"
+wait 0.5
+ignoreobstacles
+fwd 1.1 @v 0.1
+stop
+wait 1
+call "update"
+stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
+laser "$string"
+wait 1
+turn 90
+stop
+wait 0.5
+fwd 0.5 @v 0.1
+stop
+wait 0.5
gcounter = 1
label "detectLoop"
xEnd = dox[gcounter]
@@ -187,32 +222,17 @@ eval thEnd
wait 0.5
call "plan"
gcounter=gcounter+1
-if (gcounter==4) "spPoint"
wait 1
call "update"
+xCurr = xCurr-xshift
+yXurr = yCurr-yshift
+thCurr = thCurr-thshift
stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
laser "$string"
-if (gcounter<9) "detectLoop"
+if (gcounter<8) "detectLoop"
laser "zoneobst recog"
return
-label "spPoint"
-fwd -0.3 @v 0.1
-wait 1
-xEnd = 2
-yEnd = 3.4
-thEnd = -pi/2
-call "plan"
-ignoreobstacles
-fwd 1 @v 0.1
-stop
-wait 1
-call "update"
-stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
-laser "$string"
-goto "detectLoop"
-
-
% function: update
% get the robot position in world coordinates
label "update"
@@ -267,17 +287,17 @@ pno = pno-1
stringcat "getpoint p="pno
laser "$string"
wait 0.5
-xTar = $l5
-yTar = $l6
-thTar = $l7
+xTar = $l5+xshift
+yTar = $l6+yshift
+thTar = $l7+thshift
if((xTar==xOrig)&(yTar==yOrig)&(pno>0)) "loop"
call "go"
wait 2
if(pno>0) "loop"
% drive the robot to head at the target
-xTar = xEnd
-yTar = yEnd
-thTar = thEnd
+xTar = xEnd+xshift
+yTar = yEnd+yshift
+thTar = thEnd+thshift
call "go"
%wait 1
%fwd -0.2 @v 0.1:($irdistfrontmiddle<=dist)
@@ -321,7 +341,6 @@ return
% function: backToOrig
% drive the robot back to the start square
label "backToOrig"
-fwd -0.32 @v 0.1
wait 1
xEnd = 2
yEnd = 0.5
@@ -334,11 +353,10 @@ rotAng = -thCurr
turn rotAng "rad" @v 0.1:($irdistfrontleft<=0.05) | ($irdistfrontright<=0.05)
stop
wait 0.5
-fwd -1.85 @v 0.1
+fwd -1.72 @v 0.1
turn -90
ignoreobstacles
-fwd 0.05 @v 0.1
-turn 90
+fwd 0.12 @v 0.1
stop
wait 1
laser "scanset logclose"
diff --git a/objectDetect.so.0 b/objectDetect.so.0
index 1fdb22fd49c559a5a7706aaade5247eb9b2bad69..29843454db7783544546cb5b873437a5bf8e7164 100644
Binary files a/objectDetect.so.0 and b/objectDetect.so.0 differ
diff --git a/spawnLog.log b/spawnLog.log
index 13b6ef237ded9b3e122b6226230b8b3d89a33b71..e5d8a9f04beed1f2dce6e854dcf77ebbb5b0aab8 100644
--- a/spawnLog.log
+++ b/spawnLog.log
@@ -1 +1 @@
-shape: Rectangle center: [[2.31890252 1.35067841]] width: 0.4 height: 0.15 angle: 2.15942673599
\ No newline at end of file
+shape: Triangle center: [[1.62678334 1.45443528]] width: 0.3 height: 0.15 angle: -172.130057726
diff --git a/spawnObjs.py b/spawnObjs.py
index 56078ecf5152dc9105219a68fa20edb146c0bf52..eef5ed2052dfeb9684363a2dbfdd8ec00429726b 100644
--- a/spawnObjs.py
+++ b/spawnObjs.py
@@ -149,7 +149,7 @@ def spawn():
obj = Rect(BOUND, RECT_WIDTH[idx], RECT_HEIGHT[idx])
# write log files
with open('./spawnLog.log', 'w') as f:
- f.write('shape: '+shape_bin[shape]+'\tcenter: '+str(obj.origin.reshape((1,-1)))+'\twidth: '+str(obj.width)+'\theight: '+str(obj.height)+'\tangle: '+str(obj.angle/math.pi*180)+'\n')
+ f.write('shape: '+shape_bin[shape]+'\tcenter: '+str(obj.origin.reshape((1,-1)))+'\twidth: '+str(obj.width)+'\theight: '+str(obj.height)+'\tangle: '+str(obj.angle)+'\n')
if __name__ == "__main__":
spawn()
diff --git a/ulmsserver.ini b/ulmsserver.ini
index d08c8efc88d47994eb8c0bf16cbc4acfbe708155..393585648f03c43512c050d3a8b86b9fc3f4fe4e 100644
--- a/ulmsserver.ini
+++ b/ulmsserver.ini
@@ -42,4 +42,4 @@ module load="objectDetect.so.0"
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
+setinitcov Cx=0.05 Cy=0.05 Cth=0.05
\ No newline at end of file