delete

main

-%% group 16
-%% 31388 Advanced Autonomous Robots
-
-%%%%%%%%%%%%%%%%CONSTANTS%%%%%%%%%%%%%%%%%%%%%%%
-pi = 3.1415926535
-% origin
-xOrig = 0.25
-yOrig = 0.45
-% distance from the nearest node to the guidemark
-nodeDist = 0.15
-% robot length
-length  = 0.35
-% distance from the expected robot position to the guidemark
-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
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-% 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"
-
-%% 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
-
-
-%% 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
-
-%% log
-laser "scanset logopen"
-laser "scanset log=1"
-laser "odopose log=true"
-
-%%%%%%%%MAIN PROCESS%%%%%%%%%%
-%
-switch(task)
-call "subtask1"
-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: 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"
-wait 1
-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"
-gcounter = 1
-label "detectLoop"
-xEnd = dox[gcounter]
-yEnd = doy[gcounter]
-thEnd = doth[gcounter]
-eval gcounter
-eval xEnd
-eval yEnd
-eval thEnd
-wait 0.5
-call "plan"
-gcounter=gcounter+1
-if (gcounter==4) "spPoint"
-wait 1
-call "update"
-stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
-laser "$string"
-if (gcounter<9) "detectLoop"
-return
-
-label "spPoint"
-fwd -0.32 @v 0.1
-wait 1
-xEnd = 2
-yEnd = 3.4
-thEnd = -pi/2
-call "plan"
-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"
-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
-turn rotAng "rad" @v 0.1 :($cmdtime>5)|($irdistfrontmiddle<=dist)| ($irdistfrontleft<=0.05) | ($irdistfrontright<=0.05)  
-stop
-wait 2
-ignoreobstacles
-drivew xTar yTar thTar "rad" @v 0.3 :($targetdist<0.05)|($cmdtime>10)|($irdistfrontmiddle<=dist)
-stop
-wait 1
-call "update"
-% correct orientation
-rotAng = thTar-thCurr
-rotAng = normalizeanglerad(rotAng)
-stop
-wait 0.5
-turn rotAng "rad" @v 0.1 :($cmdtime>5)|($irdistfrontmiddle<=dist)| ($irdistfrontleft<=0.05) | ($irdistfrontright<=0.05)   
-stop
-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
-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
-call "go"
-%wait 1
-%fwd -0.2 @v 0.1:($irdistfrontmiddle<=dist)
-return
-
-% function: look
-% return guidemarkNumber 
-label "look"
-counter = 0
-label "scan"
-switch(counter)
-wait 3 : ($fiducialid>0)
-gmno = $fiducialid
-eval gmno
-goto "hasGotcha"
-case 1
-turn 10
-wait 3 : ($fiducialid>0)
-if(gmno==$fiducialid) "next"
-gmno = $fiducialid
-eval gmno
-"hasGotcha"
-case 2
-turn -20
-wait 3 : ($fiducialid>0)
-gmno = $fiducialid
-eval gmno
-label "hasGotcha"
-counter = counter+1
-if(counter>2) "next"
-if(gmno<0) "scan"
-return
-label "next"
-gmno = gmno+1
-if(gmno>14) "overflow"
-return
-label "overflow"
-gmno = 14
-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
-thEnd = 0
-call "plan"
-wait 1
-% to make sure it inside the square
-call "update"
-rotAng = -thCurr
-turn  rotAng "rad" @v 0.1:($irdistfrontleft<=0.05) | ($irdistfrontright<=0.05) 
-stop
-wait 0.5
-fwd -1.85 @v 0.1
-turn -90
-ignoreobstacles
-fwd 0.05 @v 0.1
-turn 90
-stop
-wait 1
-laser "scanset logclose"
-laser "odopose log=false"
-return
-
-label "end"
-stop

main.sh

-#!usr/bin/bash
-
-# config the environment
-if ! [ -x "$(command -v pip)" ]; then
-sudo apt install python-pip
-sleep 1
-pip install numpy
-pip install PyYAML
-sleep 1
-fi
-
-if ! [ -x "$(command -v gnome-terminal)" ]; then
-  sudo apt-get update
-  sudo apt-get install gnome-terminal
-  sleep 1
-fi
-
-# make sure we run the scripts in /sim folder
-if ! [ ${PWD##*/} = "sim" ];then
-   mkdir ../sim
-   cp -r . ../sim
-   cd ../sim
-fi
-
-# initialize log files
-if [ -f detectResult.log ]; then
-    rm -f detectResult.log
-fi
-
-# add object
-python spawnObjs.py
-sleep 1
-
-# 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
-## show processes
-echo "processes"
-ps -ef | grep "bash" | grep -v grep | awk '{print $2}'
-ps
-while true;do
-echo "Enter 'e' to exit"
-read flag
-if [ ! -n "$flag" ]
-then
-    echo "Invalid input"
-else
-    if [ $flag = 'e' ]
-    then
-	## clean logg files
-        rm -r *.logg
-	## restore config files
-        if [ -f 388project_temp ]; then
-	  rm -f 388project
-          mv 388project_temp 388project
-	fi
-	sleep 1
-        cd mrc_src
-	if [ -f createWalls_temp ];then
-          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  
-        break
-    fi
-fi
-done
-exit 0
-

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