Skip to content
Snippets Groups Projects
Commit a9dd7eb7 authored by manxilin's avatar manxilin
Browse files

delete

parent c733605b
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
main deleted 100644 → 0
+ 0
− 346
View file @ c733605b
%% 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 deleted 100644 → 0
+ 0
− 91
View file @ c733605b
#!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 0 → 100644
+ 18
− 0
View file @ a9dd7eb7
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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment