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

with Ruchir

parent 2c7178c4
No related branches found
No related tags found
No related merge requests found
.deps/ufunczoneobst.d ufunczoneobst.o: ufunczoneobst.cpp ufunczoneobst.h
# makefile
#
# libraryname (for shared library?)
libname = auzoneobst.so
#
# AU Robot Server location (if this is placed outside aurobotservers folder
AUROBOTDIR = ../../aurobotservers/trunk
BINDIR = ../bin
#
# preprocessor flags like -I(include dir)
CPPFLAGS = -I$(AUROBOTDIR)/include -I/usr/include/opencv2
#
# linker flags like e.g. -lpthread and -L/usr/local/lib
LDFLAGS = -g0 -shared -Wl,-soname,$(libname)
#
# extra preprocessor defines (mainly for module testcode)
DEFINES = -D LIBRARY_OPEN_NEEDED
#
# C++ compiler flags
CXXFLAGS = -g3 -O0 -Wall -pedantic -fPIC $(DEFINES)
#CXXFLAGS += -Wno-unused-but-set-variable
include ../../aurobotservers/trunk/include/opencv.mk
include ../../aurobotservers/trunk/include/opencv_flags.mk
# ifeq ($(OPENCV),2.1)
# export OPENCV=
# endif
# ifeq ($(OPENCV),)
# else
# CXXFLAGS += `pkg-config opencv-$(OPENCV) --cflags`
# endif
#
# Object files to produce before link
objects = ufunczoneobst.o
# shared library file name (version 0)
shlib = $(libname).0
# compile all - all objects depend on all other (objects)
all: $(objects)
c++ -o $(shlib) $(objects) $(LDFLAGS)
.PHONY : clean install
clean :
rm -f $(shlib) $(objects)
-@rm -fr *~ .deps
install:
cp $(shlib) ../bin
######################################################################
#
# Automatic dependencies
DEPS_MAGIC := $(shell mkdir -p .deps)
%.o: .deps/%.d
.deps/%.d: src/%.c
@cc $(CFLAGS) -MM $< | sed 's#^$(@F:%.d=%.o):#$@ $(@:.deps/%.d=%.o):#'
.deps/%.d: %.cpp
@g++ $(CFLAGS) -MM $< | sed 's#^$(@F:%.d=%.o):#$@ $(@:.deps/%.d=%.o):#' > $@
######################################################################
#
# Include automatic dependencies
-include $(patsubst %.o, .deps/%.d, $(objects))
File deleted
/***************************************************************************
* Copyright (C) 2005 by Christian Andersen and DTU *
* jca@oersted.dtu.dk *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include "ufunczoneobst.h"
#ifdef LIBRARY_OPEN_NEEDED
/**
* This function is needed by the server to create a version of this plugin */
UFunctionBase * createFunc()
{ // create an object of this type
/** replace 'UFunczoneobst' with your class name */
return new UFunczoneobst();
}
#endif
///////////////////////////////////////////////////
///////////////////////////////////////////////////
///////////////////////////////////////////////////
///////////////////////////////////////////////////
// #define SMRWIDTH 0.4
bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
{ // handle a plugin command
const int MRL = 500;
char reply[MRL];
bool ask4help;
const int MVL = 30;
char value[MVL];
ULaserData * data;
//
int i,j,imax;
double r,delta;
double minRange; // min range in meter
// double minAngle = 0.0; // degrees
// double d,robotwidth;
double zone[9];
// check for parameters - one parameter is tested for - 'help'
ask4help = msg->tag.getAttValue("help", value, MVL);
if (ask4help)
{ // create the reply in XML-like (html - like) format
sendHelpStart(msg, "zoneobst");
sendText("--- available zoneobst options\n");
sendText("help This message\n");
sendText("fake=F Fake some data 1=random, 2-4 a fake corridor\n");
sendText("device=N Laser device to use (see: SCANGET help)\n");
sendText("see also: SCANGET and SCANSET\n");
sendHelpDone();
}
else
{ // do some action and send a reply
data = getScan(msg, (ULaserData*)extra);
//
if (data->isValid())
{ // make analysis for closest measurement
minRange = 1000; // long range in meters
imax=data->getRangeCnt();
delta=imax/9.0;
for (j=0;j<9;j++)
zone[j]=minRange;
for(j=0;j<9;j++){
for (i = 0+(int)(j*delta); i < (int)((j+1)*delta); i++)
{ // range are stored as an integer in current units
r = data->getRangeMeter(i);
if (r >= 0.020)
{ // less than 20 units is a flag value for URG scanner
if (r<zone[j])
zone[j]=r;
}
}
}
/* SMRCL reply format */
snprintf(reply, MRL, "<laser l0=\"%g\" l1=\"%g\" l2=\"%g\" l3=\"%g\" l4=\"%g\" "
"l5=\"%g\" l6=\"%g\" l7=\"%g\" l8=\"%g\" />\n",
zone[0],zone[1],zone[2],zone[3],zone[4],
zone[5],zone[6],zone[7],zone[8]);
// send this string as the reply to the client
sendMsg(msg, reply);
// save also as gloabl variable
for(i = 0; i < 9; i++)
var_zone->setValued(zone[i], i);
}
else
sendWarning(msg, "No scandata available");
}
// return true if the function is handled with a positive result
// used if scanpush or push has a count of positive results
return true;
}
void UFunczoneobst::createBaseVar()
{ // add also a global variable (global on laser scanner server) with latest data
var_zone = addVarA("zone", "0 0 0 0 0 0 0 0 0", "d", "Value of each laser zone. Updated by zoneobst.");
}
/***************************************************************************
* Copyright (C) 2005 by Christian Andersen and DTU *
* jca@oersted.dtu.dk *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#ifndef UFUNC_MRCOBST_H
#define UFUNC_MRCOBST_H
#include <cstdlib>
#include <ulms4/ufunclaserbase.h>
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
/**
* Laserscanner function to demonstrate
* simple laser scanner data handling and analysis
* @author Christian Andersen
*/
class UFunczoneobst : public UFuncLaserBase
{
public:
/**
Constructor */
UFunczoneobst()
{ // set the command (or commands) handled by this plugin
setCommand("zoneobst", "zoneobstif", "obstacle detect for MRC (Compiled " __DATE__ " " __TIME__ ")");
createBaseVar();
}
/**
Handle incomming command
(intended for command separation)
Must return true if the function is handled -
otherwise the client will get a failed - reply */
virtual bool handleCommand(UServerInMsg * msg, void * extra);
protected:
void createBaseVar();
UVariable *var_zone;
};
#endif
File deleted
File moved
This diff is collapsed.
#!usr/bin/bash #!usr/bin/bash
# install gnome-terminal
# config the environment
if ! [ -x "$(command -v pip)" ]; then if ! [ -x "$(command -v pip)" ]; then
sudo apt install python-pip sudo apt install python-pip
sleep 1 sleep 1
...@@ -7,30 +8,49 @@ pip install numpy ...@@ -7,30 +8,49 @@ pip install numpy
pip install PyYAML pip install PyYAML
sleep 1 sleep 1
fi fi
if ! [ -x "$(command -v gnome-terminal)" ]; then if ! [ -x "$(command -v gnome-terminal)" ]; then
sudo apt-get update sudo apt-get update
sudo apt-get install gnome-terminal sudo apt-get install gnome-terminal
sleep 1 sleep 1
fi fi
# terminal 0: add object
# 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 laserlogs.log ]; then
rm -f laserlogs.log
fi
# add object
python spawnObjs.py python spawnObjs.py
sleep 1 sleep 1
# terminal 1: laser server # terminal 1: laser server
gnome-terminal -t "laser server" -x bash -c "cd /home/smr/sim/bash_src/; gnome-terminal -t "laser server" -x bash -c "cd /home/smr/sim/bash_src/;
sh laser.sh;exec bash;" sh laser.sh;exec bash;"
sleep 1 sleep 1
# terminal 2: run the simulator # terminal 2: run the simulator
gnome-terminal -t "simulator" -x bash -c "cd /home/smr/sim/bash_src/; gnome-terminal -t "simulator" -x bash -c "cd /home/smr/sim/bash_src/;
sh sim.sh;exec bash;" sh sim.sh;exec bash;"
sleep 1 sleep 1
# terminal 3: run the client # terminal 3: run the client
gnome-terminal -t "client" -x bash -c "cd /home/smr/sim/bash_src/; gnome-terminal -t "client" -x bash -c "cd /home/smr/sim/bash_src/;
sh client.sh;exec bash;" sh client.sh;exec bash;"
sleep 1 sleep 1
# terminal 4: run mrc scripts # terminal 4: run mrc scripts
gnome-terminal -t "mrc" -x bash -c "cd /home/smr/sim/bash_src/; gnome-terminal -t "mrc" -x bash -c "cd /home/smr/sim/bash_src/;
sh mov.sh;exec bash;" sh mov.sh;exec bash;"
sleep 1 sleep 1
# terminate # terminate
## show processes ## show processes
echo "processes" echo "processes"
...@@ -39,24 +59,23 @@ ps ...@@ -39,24 +59,23 @@ ps
while true;do while true;do
echo "Enter 'e' to exit" echo "Enter 'e' to exit"
read flag read flag
echo $flag
if [ ! -n "$flag" ] if [ ! -n "$flag" ]
then then
echo "Invalid input" echo "Invalid input"
else else
if [ $flag=='e' ] if [ $flag = 'e' ]
then then
## clean logg files ## clean logg files
rm -r *.logg rm -r *.logg
## restore config files ## restore config files
if [ -f 388project_temp ]; then if [ -f 388project_temp ]; then
rm -r 388project rm -f 388project
mv 388project_temp 388project mv 388project_temp 388project
fi fi
sleep 1 sleep 1
cd mrc_src cd mrc_src
if [ -f createWalls_temp ];then if [ -f createWalls_temp ];then
rm -r createWalls rm -f createWalls
mv createWalls_temp createWalls mv createWalls_temp createWalls
fi fi
## kill all terminal processes ## kill all terminal processes
......
Debug file for the robot initialization Debug file for the robot initialization
--------------------------------------- ---------------------------------------
Tue May 26 19:14:40 2020 Wed May 27 18:58:01 2020
--------------------------------------- ---------------------------------------
Module : robotinfo Module : robotinfo
......
...@@ -18,7 +18,7 @@ gmno = 13 ...@@ -18,7 +18,7 @@ gmno = 13
%% - 1: subtask1, follow the guidemarks %% - 1: subtask1, follow the guidemarks
%% - 2: subtask2, recognize unknown object %% - 2: subtask2, recognize unknown object
%% - 0: finish the full task, subtask1->subtask2 %% - 0: finish the full task, subtask1->subtask2
task = 1 task = 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% prior guidemark information % prior guidemark information
...@@ -76,6 +76,36 @@ gmth[14]=0 ...@@ -76,6 +76,36 @@ gmth[14]=0
%% get cost for graph planner %% get cost for graph planner
laser "calculatecost" laser "calculatecost"
%% path for detecting the object
array "dox" 8
array "doy" 8
array "doth" 8
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
doy[8] = 0.5
doth[8] = pi/4
%% set initial odopose %% set initial odopose
set "$odox" 0 set "$odox" 0
set "$odoy" 0 set "$odoy" 0
...@@ -105,7 +135,7 @@ call "backToOrig" ...@@ -105,7 +135,7 @@ call "backToOrig"
goto "end" goto "end"
case 2 case 2
call "subtask2" call "subtask2"
call "backToOrig" %call "backToOrig"
goto "end" goto "end"
%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%
...@@ -144,7 +174,32 @@ goto "top" ...@@ -144,7 +174,32 @@ goto "top"
% function: subtask2 % function: subtask2
% recoggnize unknow objects % recoggnize unknow objects
label "subtask2" label "subtask2"
counter = 1
label "detectLoop"
xEnd = dox[counter]
yEnd = doy[counter]
thEnd = doth[counter]
wait 0.5
call "plan"
counter=counter+1
if (counter==4) "spPoint"
wait 1
laser "zoneobst logOn"
if (counter<9) "detectLoop"
return 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
laser "zoneobst logOn"
goto "detectLoop"
% function: update % function: update
% get the robot position in world coordinates % get the robot position in world coordinates
...@@ -278,6 +333,5 @@ laser "scanset logclose" ...@@ -278,6 +333,5 @@ laser "scanset logclose"
laser "odopose log=false" laser "odopose log=false"
return return
label "end" label "end"
stop stop
...@@ -12,7 +12,7 @@ laser "addpoint pno=5 x=3.5 y=1.5" ...@@ -12,7 +12,7 @@ laser "addpoint pno=5 x=3.5 y=1.5"
laser "addpoint pno=6 x=4.5 y=1.5" laser "addpoint pno=6 x=4.5 y=1.5"
laser "addpoint pno=7 x=-0.5 y=2.5" laser "addpoint pno=7 x=-0.5 y=2.5"
laser "addpoint pno=8 x=0.5 y=2.5" laser "addpoint pno=8 x=0.5 y=2.5"
laser "addpoint pno=9 x=2 y=2" laser "addpoint pno=9 x=2 y=2.2"
laser "addpoint pno=10 x=3.5 y=2.5" laser "addpoint pno=10 x=3.5 y=2.5"
laser "addpoint pno=11 x=4.5 y=2.5" laser "addpoint pno=11 x=4.5 y=2.5"
laser "addpoint pno=12 x=-0.5 y=3.5" laser "addpoint pno=12 x=-0.5 y=3.5"
...@@ -29,13 +29,13 @@ laser "addpoint pno=22 x=3.5 y=4.5" ...@@ -29,13 +29,13 @@ laser "addpoint pno=22 x=3.5 y=4.5"
laser "addpoint pno=23 x=1.5 y=4" laser "addpoint pno=23 x=1.5 y=4"
laser "addpoint pno=24 x=2.5 y=4" laser "addpoint pno=24 x=2.5 y=4"
laser "addpoint pno=25 x=2 y=0.5" laser "addpoint pno=25 x=2 y=0.5"
laser "addpoint pno=26 x=1 y=2.5"
laser "addpoint pno=27 x=3 y=2.5"
laser "addpoint pno=28 x=1 y=0.5"
laser "addpoint pno=29 x=3 y=0.5"
%%% add connections %%% add connections
laser "addcon pno1=1 pno2=25"
laser "addcon pno1=25 pno2=1"
laser "addcon pno1=1 pno2=4" laser "addcon pno1=1 pno2=4"
laser "addcon pno1=2 pno2=25"
laser "addcon pno1=25 pno2=2"
laser "addcon pno1=2 pno2=5" laser "addcon pno1=2 pno2=5"
laser "addcon pno1=3 pno2=7" laser "addcon pno1=3 pno2=7"
laser "addcon pno1=4 pno2=1" laser "addcon pno1=4 pno2=1"
...@@ -48,13 +48,13 @@ laser "addcon pno1=7 pno2=8" ...@@ -48,13 +48,13 @@ laser "addcon pno1=7 pno2=8"
laser "addcon pno1=7 pno2=12" laser "addcon pno1=7 pno2=12"
laser "addcon pno1=8 pno2=4" laser "addcon pno1=8 pno2=4"
laser "addcon pno1=8 pno2=7" laser "addcon pno1=8 pno2=7"
laser "addcon pno1=8 pno2=9" %laser "addcon pno1=8 pno2=9"
laser "addcon pno1=8 pno2=13" laser "addcon pno1=8 pno2=13"
laser "addcon pno1=9 pno2=8" %laser "addcon pno1=9 pno2=8"
laser "addcon pno1=9 pno2=10" %laser "addcon pno1=9 pno2=10"
laser "addcon pno1=9 pno2=15" laser "addcon pno1=9 pno2=15"
laser "addcon pno1=10 pno2=5" laser "addcon pno1=10 pno2=5"
laser "addcon pno1=10 pno2=9" %laser "addcon pno1=10 pno2=9"
laser "addcon pno1=10 pno2=11" laser "addcon pno1=10 pno2=11"
laser "addcon pno1=10 pno2=17" laser "addcon pno1=10 pno2=17"
laser "addcon pno1=11 pno2=6" laser "addcon pno1=11 pno2=6"
...@@ -82,3 +82,19 @@ laser "addcon pno1=19 pno2=23" ...@@ -82,3 +82,19 @@ laser "addcon pno1=19 pno2=23"
laser "addcon pno1=20 pno2=24" laser "addcon pno1=20 pno2=24"
laser "addcon pno1=23 pno2=19" laser "addcon pno1=23 pno2=19"
laser "addcon pno1=24 pno2=20" laser "addcon pno1=24 pno2=20"
laser "addcon pno1=1 pno2=28"
laser "addcon pno1=28 pno2=1"
laser "addcon pno1=28 pno2=25"
laser "addcon pno1=25 pno2=28"
laser "addcon pno1=29 pno2=25"
laser "addcon pno1=25 pno2=29"
laser "addcon pno1=2 pno2=29"
laser "addcon pno1=29 pno2=2"
laser "addcon pno1=8 pno2=26"
laser "addcon pno1=26 pno2=8"
laser "addcon pno1=9 pno2=26"
laser "addcon pno1=26 pno2=9"
laser "addcon pno1=9 pno2=27"
laser "addcon pno1=27 pno2=9"
laser "addcon pno1=10 pno2=27"
laser "addcon pno1=27 pno2=10"
\ No newline at end of file
File deleted
File added
shape: Triangle center: [[2.57248992 1.74175571]] width: 0.3 height: 0.2 angle: 2.11407176003 shape: Triangle center: [[2.68476934 1.56584345]] width: 0.3 height: 0.15 angle: 2.60124991477
\ No newline at end of file \ No newline at end of file
...@@ -30,10 +30,6 @@ def read_config(config_path): ...@@ -30,10 +30,6 @@ def read_config(config_path):
cfg = yaml.load(f.read(), Loader=yaml.FullLoader) cfg = yaml.load(f.read(), Loader=yaml.FullLoader)
return cfg or {} return cfg or {}
# initialize logfile
if os.path.exists('./spawnLog.log'):
os.remove('./spawnLog.log')
# create temp files for revision # create temp files for revision
shutil.copy(PROJ_NAME, PROJ_NAME+'_temp') shutil.copy(PROJ_NAME, PROJ_NAME+'_temp')
shutil.copy(MRC_NAME, MRC_NAME+'_temp') shutil.copy(MRC_NAME, MRC_NAME+'_temp')
...@@ -136,7 +132,7 @@ class Trian(MyObj): ...@@ -136,7 +132,7 @@ class Trian(MyObj):
def spawn(): def spawn():
# read confiugrations # read confiugrations
cfg = read_config('./objConfig.yaml') cfg = read_config('./config/objConfig.yaml')
RANDOM = cfg['RANDOM'] RANDOM = cfg['RANDOM']
INDEX = cfg['INDEX'] INDEX = cfg['INDEX']
shape_bin = ['Rectangle','Triangle'] shape_bin = ['Rectangle','Triangle']
...@@ -149,12 +145,12 @@ def spawn(): ...@@ -149,12 +145,12 @@ def spawn():
shape = INDEX//3 shape = INDEX//3
idx = 1-INDEX%2 idx = 1-INDEX%2
if shape: if shape:
obj = Trian(BOUND, TRIAN_WIDTH[idx], TRIAN_HEIGHT[idx])
else:
# rectangle # rectangle
obj = Rect(BOUND, RECT_WIDTH[idx], RECT_HEIGHT[idx]) obj = Rect(BOUND, RECT_WIDTH[idx], RECT_HEIGHT[idx])
else:
obj = Trian(BOUND, TRIAN_WIDTH[idx], TRIAN_HEIGHT[idx])
# write log files # write log files
with open('./spawnLog.log', 'a+') as f: 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)) 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))
if __name__ == "__main__": if __name__ == "__main__":
......
/***************************************************************************
* Copyright (C) 2005 by Christian Andersen and DTU *
* jca@oersted.dtu.dk *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include "ufunczoneobst.h"
#ifdef LIBRARY_OPEN_NEEDED
/**
* This function is needed by the server to create a version of this plugin */
UFunctionBase * createFunc()
{ // create an object of this type
/** replace 'UFunczoneobst' with your class name */
return new UFunczoneobst();
}
#endif
bool UFunczoneobst::setResource(UResBase * resource, bool remove) { // load resource as provided by the server (or other plugins)
bool result = true;
if (resource->isA(UResPoseHist::getOdoPoseID())) { // pointer to server the resource that this plugin can provide too
// but as there might be more plugins that can provide the same resource
// use the provided
if (remove)
// the resource is unloaded, so reference must be removed
poseHist = NULL;
else if (poseHist != (UResPoseHist *) resource)
// resource is new or is moved, save the new reference
poseHist = (UResPoseHist *) resource;
else
// reference is not used
result = false;
}
// other resource types may be needed by base function.
result = UFunctionBase::setResource(resource, remove);
return result;
}
///////////////////////////////////////////////////
///////////////////////////////////////////////////
///////////////////////////////////////////////////
///////////////////////////////////////////////////
// #define SMRWIDTH 0.4
bool UFunczoneobst::handleCommand(UServerInMsg * msg, void * extra)
{ // handle a plugin command
const int MRL = 500;
char reply[MRL];
bool ask4help;
const int MVL = 30;
char value[MVL];
ULaserData * data;
double robotwidth;
//
int i,j,imax;
double r,delta;
double minRange; // min range in meter
// double minAngle = 0.0; // degrees
// double d,robotwidth;
double zone[9];
// check for parameters - one parameter is tested for - 'help'
ask4help = msg->tag.getAttValue("help", value, MVL);
if (ask4help)
{ // create the reply in XML-like (html - like) format
sendHelpStart(msg, "zoneobst");
sendText("--- available zoneobst options\n");
sendText("help This message\n");
sendText("fake=F Fake some data 1=random, 2-4 a fake corridor\n");
sendText("device=N Laser device to use (see: SCANGET help)\n");
sendText("see also: SCANGET and SCANSET\n");
sendHelpDone();
}
else
{ // do some action and send a reply
data = getScan(msg, (ULaserData*)extra);
//
if (data->isValid())
{
// check if a attribute (parameter) with name width exists and if so get its value
bool gotwidth = msg->tag.getAttValue("width", value, MVL);
if (gotwidth) {
robotwidth=strtod(value, NULL);
}
else {
robotwidth=0.26;
}
UPose poseAtScan = poseHist->getPoseAtTime(data->getScanTime());
// Gets the odometry pose at the time when the laserscan was taken, poseAtScan.x poseAtScan.y poseAtScan.a (x,y,angle)
// make analysis for closest measurement
minRange = 1000; // long range in meters
imax=data->getRangeCnt();
delta=imax/9.0;
for (j=0;j<9;j++)
zone[j]=minRange;
for(j=0;j<9;j++){
for (i = 0+(int)(j*delta); i < (int)((j+1)*delta); i++)
{ // range are stored as an integer in current units
r = data->getRangeMeter(i);
if (r >= 0.020)
{ // less than 20 units is a flag value for URG scanner
if (r<zone[j])
zone[j]=r;
}
}
}
/* SMRCL reply format */
//snprintf(reply, MRL, "<laser l10=\"%g\" l11=\"%g\" l12=\"%g\" l13=\"%g\" l14=\"%g\" "
// "l15=\"%g\" l16=\"%g\" l17=\"%g\" l18=\"%g\" />\n",
snprintf(reply, MRL, "<laser l1=\"%g\" l1=\"%g\" l2=\"%g\" l3=\"%g\" l4=\"%g\" "
"l5=\"%g\" l6=\"%g\" l7=\"%g\" l8=\"%g\" />\n",
zone[0],zone[1],zone[2],zone[3],zone[4],
zone[5],zone[6],zone[7],zone[8]);
// send this string as the reply to the client
sendMsg(msg, reply);
// save also as gloabl variable
for(i = 0; i < 9; i++)
var_zone->setValued(zone[i], i);
}
else
sendWarning(msg, "No scandata available");
}
// return true if the function is handled with a positive result
// used if scanpush or push has a count of positive results
return true;
}
void UFunczoneobst::createBaseVar()
{ // add also a global variable (global on laser scanner server) with latest data
var_zone = addVarA("zone", "0 0 0 0 0 0 0 0 0", "d", "Value of each laser zone. Updated by zoneobst.");
}
/***************************************************************************
* Copyright (C) 2005 by Christian Andersen and DTU *
* jca@oersted.dtu.dk *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#ifndef UFUNC_MRCOBST_H
#define UFUNC_MRCOBST_H
#include <cstdlib>
#include <ulms4/ufunclaserbase.h>
#include <urob4/uresposehist.h>
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
/**
* Laserscanner function to demonstrate
* simple laser scanner data handling and analysis
* @author Christian Andersen
*/
class UFunczoneobst : public UFuncLaserBase
{
public:
/**
Constructor */
UFunczoneobst()
{ // set the command (or commands) handled by this plugin
setCommand("zoneobst", "zoneobstif", "obstacle detect for MRC (Compiled " __DATE__ " " __TIME__ ")");
createBaseVar();
}
virtual bool setResource(UResBase * resource, bool remove);
/**
Handle incomming command
(intended for command separation)
Must return true if the function is handled -
otherwise the client will get a failed - reply */
virtual bool handleCommand(UServerInMsg * msg, void * extra);
protected:
void createBaseVar();
UVariable *var_zone;
UResPoseHist * poseHist;
};
#endif
...@@ -36,7 +36,8 @@ scanset log=used ...@@ -36,7 +36,8 @@ scanset log=used
## log navigation module ## log navigation module
module load="aupoly.so.0" module load="aupoly.so.0"
module load="aulocalize.so.0" module load="aulocalize.so.0"
module load='auplan.so.0' module load="auplan.so.0"
module load="objectDetect.so.0"
## initialize graph planner ## initialize graph planner
resetplanner resetplanner
## set Kalman parameters ## set Kalman parameters
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment