final

config/objConfig.yaml

 # RANDOM: bool ,if spawn objects randomly
-RANDOM: True
+RANDOM: False
 # INDEX: 1-4, assign the index of the object, only works when RANDOM=False
 # index 1: Rectangle, width=0.4, height=0.15 
 # index 2: Rectangle, width=0.3, height=0.2
 # index 3: Triangle, width=0.4, height=0.1
 # index 4: Triangle, width=0.3, height=0.15
-INDEX: 1
\ No newline at end of file
+INDEX: 4
\ No newline at end of file

cpp_src/ufunczoneobst.cpp

@@ -128,6 +128,12 @@ bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
   }  
   else if(ifRecog)
   {
+    msg->tag.getAttValue("xshift", value, MVL);
+    double xshift = strtod(value, NULL);
+    msg->tag.getAttValue("yshift", value, MVL);
+    double yshift = strtod(value, NULL);
+    //msg->tag.getAttValue("thshift", value, MVL);
+    //double thshift = strtod(value, NULL);
     getPoints();
     // output the info on the object
     // origion coordinates, width, height, orientation
@@ -217,6 +223,8 @@ bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
           angle = calcAngle(0,2);
         }
       }
+      center_x+=xshift;
+      center_y+=yshift;
       cout<<"angle "<<angle<<endl;
       cout<<"origin"<<center_x<<'\t'<<center_y<<endl;
       cout<<width<<'\t'<<height<<'\n';
@@ -241,6 +249,8 @@ bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
       // get width ang height of the object
       double width = dist[2];
       double height = dist[0];
+      center_x+=xshift;
+      center_y+=yshift;
       cout<<"origin"<<center_x<<'\t'<<center_y<<endl;
       cout<<width<<'\t'<<height<<'\n';
       // get angle

detectResult.log

-shape: Triangle	center: [1.63446	,1.45036]	width: 0.315473	height: 0.136582	angle: -172.468	

mrc_src/data.debug

 Debug file for the robot initialization
 ---------------------------------------
-Sun May 31 15:13:12 2020
+Mon Jun  1 20:15:01 2020
 ---------------------------------------
 
 Module : robotinfo

mrc_src/main

@@ -13,7 +13,7 @@ length  = 0.35
 % distance from the expected robot position to the guidemark
 dist = 0.5-0.26
 % the first guidemark number
-gmno = 13
+gmno = 2
 % shift value
 xshift = 0
 yshift = 0
@@ -224,13 +224,17 @@ call "plan"
 gcounter=gcounter+1
 wait 1
 call "update"
-xCurr = xCurr-xshift
-yXurr = yCurr-yshift
-thCurr = thCurr-thshift
+%xCurr = xCurr-xshift
+%yXurr = yCurr-yshift
+%thCurr = thCurr-thshift
 stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
 laser "$string"
 if (gcounter<8) "detectLoop"
-laser "zoneobst recog"
+xshift = -xshift
+yshift = -yshift
+thshift = -thshift
+stringcat "zoneobst recog xshift="xshift" yshift="yshift" thshift="thshift
+laser "$string"
 return
 
 % function: update
@@ -342,26 +346,24 @@ return
 % drive the robot back to the start square
 label "backToOrig"
 wait 1
-xEnd = 2
-yEnd = 0.5
-thEnd = 0
-call "plan"
-wait 1
-% to make sure it inside the square
+turn 45 @v 0.1
+stop
+wait 0.5
 call "update"
-rotAng = -thCurr
-turn  rotAng "rad" @v 0.1:($irdistfrontleft<=0.05) | ($irdistfrontright<=0.05) 
+dist = 0.22-xCurr
+fwd dist @v 0.1
 stop
 wait 0.5
-fwd -1.72 @v 0.1
 turn -90
+stop  
+wait 0.5
 ignoreobstacles
-fwd 0.12 @v 0.1
+drive @v 0.1 :($irdistfrontmiddle<=0.2)
 stop
-wait 1
-laser "scanset logclose"
-laser "odopose log=false"
 return
 
 label "end"
+wait 1
+laser "scanset logclose"
+laser "odopose log=false"
 stop

plotScanData.m

-clear all; close all; clc;
-pose = load('pose.txt');
-P = [];
-for j = 1:8
-    new_point = zeros(361,2);
-    name = 'log'+string(j)+'.txt';
-    log = load(name);
-    p = pose(j,:);
-    for i = 1:361
-        new_point(i,1) = (log(i,1)+0.26)*cos(p(3))-log(i,2)*sin(p(3))+p(1);
-        new_point(i,2) = (log(i,1)+0.26)*sin(p(3))+log(i,2)*cos(p(3))+p(2);
-        P = [new_point;P];
-    end
-end
-%scatter(log5(:,1), log5(:,2));
-scatter(P(:,1), P(:,2));
-%xlim([1,3]);
-%ylim([1,2]);
\ No newline at end of file

spawnLog.log

-shape: Triangle	center: [[1.62678334 1.45443528]]	width: 0.3	height: 0.15	angle: -172.130057726
+shape: Triangle	center: [[1.76920511 1.43333006]]	width: 0.3	height: 0.15	angle: 2.27451326113

splitLog.py

-import os
-if os.path.exists('pose.txt'):
-    os.remove('pose.txt')
-with open('laserlogs.log', 'r') as f:
-    txt = f.read()
-log = txt.split(';')
-log = log[:-1]
-for i in range(len(log)):
-    filename = 'log'+str(i+1)+'.txt'
-    txt = log[i]
-    txt = txt.split('\n')
-    pose = txt[0] if txt[0] else txt[1]
-    laser = txt[1:] if txt[0] else txt[2:]
-    laser = '\n'.join(laser)
-    with open(filename, 'w+') as f:
-        f.write(laser)
-    with open('pose.txt', 'a+') as f:
-        f.write(pose+'\n')
\ No newline at end of file