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

update

parent af41dc15
No related branches found
No related tags found
No related merge requests found
# makefile
#
# libraryname (for shared library?)
libname = objectDetect.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))
#!/usr/bin/bash
cd ~/home/smr/mobotware/aurs-plugins/auzoneobst/
make clean
make
cp objectDetect.so.0 /home/smr/sim/
/***************************************************************************
* 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];
vector<double> x_laser, y_laser;
//---------useful commands
//bool ask4help=false;
bool ifDetect=false;
bool ifRecog = false;
//------------------------
const int MVL = 30;
char value[MVL];
ULaserData * data;
double pose[3]={0};
//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);
ifDetect = msg->tag.getAttValue("detect", value, MVL);
ifRecog = msg->tag.getAttValue("recog", 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
fov=data->getRangeCnt();
delta=fov/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 global variable
for(i = 0; i < 9; i++)
var_zone->setValued(zone[i], i);
}
else
sendWarning(msg, "No scandata available");
}
*/
if (ifDetect)
{
// store data from laser in a log file
data = getScan(msg, (ULaserData*)extra);
// get data from the laser
scanit(data, x_laser, y_laser);
// get current pose
msg->tag.getAttValue("x", value, MVL);
pose[0] = strtod(value, NULL);
msg->tag.getAttValue("y", value, MVL);
pose[1] = strtod(value, NULL);
msg->tag.getAttValue("th", value, MVL);
pose[2] = strtod(value, NULL);
// transform the coordinates from laser frame to world frame
transform(x_laser, y_laser, pose);
ifDetect = false;
}
else if(ifRecog)
{
getPoints();
// output the info on the object
// origion coordinates, width, height, orientation
if (vex_x.size()==3)
{
// if there are 3 vex points, then triangle
cout<<"Triangle"<<endl;
vector<double> dist;
double center_x = 0;
double center_y = 0;
double width = 0;
double height = 0;
double angle = 0;
int oriIdx = 0;
dist.push_back(calcDist(vex_x[0], vex_y[0], vex_x[1], vex_y[1]));
dist.push_back(calcDist(vex_x[1], vex_y[1], vex_x[2], vex_y[2]));
dist.push_back(calcDist(vex_x[2], vex_y[2], vex_x[0], vex_y[0]));
vector<double>::iterator p = min_element(dist.begin(),dist.end());
double dist_min = *p;
p = max_element(dist.begin(),dist.end());
double dist_max = *p;
// get width ang height of the object
for (int i = 0;(double)i<dist.size();i++)
{
if(dist[i]==dist_min)
{
height = dist[i];
}
if((dist[i]!=dist_min) & (dist[i]!=dist_max))
{
width = dist[i];
}
}
// determine origin coordinates
if (((dist[0]==width) & (dist[2]==height)) | ((dist[0]==height) & (dist[2]==width)))
{
center_x = vex_x[0];
center_y = vex_y[0];
oriIdx = 0;
}
if (((dist[0]==width) & (dist[1]==height)) | ((dist[0]==height) & (dist[1]==width)))
{
center_x = vex_x[1];
center_y = vex_y[1];
oriIdx = 1;
}
if (((dist[1]==width) & (dist[2]==height)) | ((dist[1]==height) & (dist[2]==width)))
{
center_x = vex_x[2];
center_y = vex_y[2];
oriIdx = 2;
}
// calculate rotation angle
if (dist[0]==width)
{
// then we use the line connecting point 0 and 1
if(oriIdx==0)
{
angle = calcAngle(0,1);
}
else
{
angle = calcAngle(1,0);
}
}
if (dist[1]==width)
{
// then we use the line connecting point 1 and 2
if(oriIdx==1)
{
angle = calcAngle(1,2);
}
else
{
angle = calcAngle(2,1);
}
}
if (dist[2]==width)
{
// then we use the line connecting point 0 and 2
if(oriIdx==2)
{
angle = calcAngle(2,0);
}
else
{
angle = calcAngle(0,2);
}
}
cout<<"angle "<<angle<<endl;
cout<<"origin"<<center_x<<'\t'<<center_y<<endl;
cout<<width<<'\t'<<height<<'\n';
// write logs
ofstream file("detectResult.log", ios::trunc);
file << "shape: Triangle" << '\t'<<"center: ["<<center_x<<'\t'<<","<<center_y<<']'<<'\t'<<"width: "<<width<<'\t'<<"height: "<<height<<'\t'<<"angle: "<<angle<<'\t'<<'\n';
file.close();
}
else if (vex_y.size()==4)
{
cout<<"Rectangle"<<endl;
vector<double> dist;
double angle1, angle2, angle;
// determine origin coordinares
double center_x = (vex_x[0]+vex_x[1]+vex_x[2]+vex_x[3])/4;
double center_y = (vex_y[0]+vex_y[1]+vex_y[2]+vex_y[3])/4;
dist.push_back(calcDist(vex_x[0], vex_y[0], vex_x[2], vex_y[2]));
dist.push_back(calcDist(vex_x[0], vex_y[0], vex_x[3], vex_y[3]));
dist.push_back(calcDist(vex_x[1], vex_y[1], vex_x[2], vex_y[2]));
dist.push_back(calcDist(vex_x[1], vex_y[1], vex_x[3], vex_y[3]));
sort(dist);
// get width ang height of the object
double width = dist[2];
double height = dist[0];
cout<<"origin"<<center_x<<'\t'<<center_y<<endl;
cout<<width<<'\t'<<height<<'\n';
// get angle
if(calcDist(vex_x[0], vex_y[0], vex_x[2], vex_y[2]) == width)
{
angle1 = calcAngle(0,2);
if(angle1<0)
{angle1 += 180;}
angle2 = calcAngle(1,3);
if(angle2<0)
{angle2 += 180;}
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;}
angle2 = calcAngle(1,2);
if(angle2<0)
{angle2 += 180;}
angle = (angle1+angle2)/2;
}
cout<<"angle: "<<angle<<endl;
// write logs
ofstream file("detectResult.log", ios::trunc);
file << "shape: Rectangle" << '\t'<<"center: ["<<center_x<<'\t'<<","<<center_y<<']'<<'\t'<<"width: "<<width<<'\t'<<"height: "<<height<<'\t'<<"angle: "<<angle<<'\t'<<'\n';
file.close();
}
ifRecog = false;
}
// 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.");
}
void UFunczoneobst::scanit(ULaserData * data, vector<double> &x_laser, vector<double> &y_laser)
/**
* call laser scan
**/
{
// field of view of the laser
int fov = data->getRangeCnt();
double maxDist = 1000;
double dist = 0;
//cout<<"fov is\t"<<fov<<endl;
// loop over the angles
// get point Cartesian coordinates
for(int i = 0; i<fov; i++)
{
dist = data->getRangeMeter(i);
dist = dist>maxDist?maxDist:dist;
x_laser.push_back(dist*cos(data->getAngleRad(i)));
y_laser.push_back(dist*sin(data->getAngleRad(i)));
}
}
void UFunczoneobst::transform(vector<double> &x_laser, vector<double> &y_laser, double pose[3])
/**
* transfrom the point coordinates from laser frame to world frame
* Input:
* pose: the robot pose
* x_laser, y_laser: the coordinates in laser frame
**/
{
double x,y,theta, x_w, y_w;
x = pose[0];
y = pose[1];
theta = pose[2];
x += 0.26*cos(theta);
y += 0.26*sin(theta);
for(int i=0;(double)i<x_laser.size();i++)
{
// the laser pose is (0.26, 0, 0) in robot frame
// transform from laser frame to world frame
x_w = x_laser[i]*cos(theta)-y_laser[i]*sin(theta)+x;
y_w = x_laser[i]*sin(theta)+y_laser[i]*cos(theta)+y;
// we only need the points in the target region
if ((x_w>=1) & (x_w<=3) & (y_w>=1) & (y_w<=2))
{
point_x.push_back(x_w);
point_y.push_back(y_w);
}
}
}
void UFunczoneobst::getPoints()
/**
* get the left-most, right-most, upper-most and lower-most point in a set of points
**/
{
vector<double>::iterator p = min_element(point_x.begin(), point_x.end());
double min_x = *p;
p = max_element(point_x.begin(), point_x.end());
double max_x = *p;
p = min_element(point_y.begin(), point_y.end());
double min_y = *p;
p = max_element(point_y.begin(), point_y.end());
double max_y = *p;
int left = -1;
int right = -1;
int lower = -1;
int upper = -1;
vector<double> cand_x, cand_y;
// find the 4 points
for(int i = 0;(double)i<point_x.size();i++)
{
//left
if(point_x[i]==min_x)
{
if((left==-1))
{
left = i;
}
else
{
left = -2;
}
cout<<"left "<<i<<'\t'<<point_x[i]<<'\t'<<point_y[i]<<endl;
}
//right
if(point_x[i]==max_x)
{
if((right==-1))
{
right = i;
}
else
{
right = -2;
}
cout<<"right "<<i<<'\t'<<point_x[i]<<'\t'<<point_y[i]<<endl;
}
//lower
if(point_y[i]==min_y)
{
if((lower==-1))
{
lower = i;
}
else
{
lower = -2;
}
cout<<"lower "<<i<<'\t'<<point_x[i]<<'\t'<<point_y[i]<<endl;
}
//upper
if(point_y[i]==max_y)
{
if((upper==-1))
{
upper = i;
}
else
{
upper = -2;
}
cout<<"upper "<<i<<'\t'<<point_x[i]<<'\t'<<point_y[i]<<endl;
}
}
if((left==-2)&(right==-2)&(lower==-2)&(upper==-2))
{
vex_x.push_back(min_x);
vex_x.push_back(min_x);
vex_x.push_back(max_x);
vex_x.push_back(max_x);
vex_y.push_back(min_y);
vex_y.push_back(max_y);
vex_y.push_back(min_y);
vex_y.push_back(max_y);
}else if((left==-2)&(lower==-2))
{
vex_x.push_back(min_x);
vex_x.push_back(min_x);
vex_x.push_back(max_x);
vex_y.push_back(min_y);
vex_y.push_back(max_y);
vex_y.push_back(min_y);
}else if((right==-2)&(lower==-2))
{
vex_x.push_back(max_x);
vex_x.push_back(max_x);
vex_x.push_back(min_x);
vex_y.push_back(min_y);
vex_y.push_back(max_y);
vex_y.push_back(min_y);
}else if((right==-2)&(upper==-2))
{
vex_x.push_back(min_x);
vex_x.push_back(max_x);
vex_x.push_back(max_x);
vex_y.push_back(max_y);
vex_y.push_back(min_y);
vex_y.push_back(max_y);
}else if((left==-2)&(upper==-2))
{
vex_x.push_back(min_x);
vex_x.push_back(min_x);
vex_x.push_back(max_x);
vex_y.push_back(min_y);
vex_y.push_back(max_y);
vex_y.push_back(max_y);
}else
{
cand_x.push_back(point_x[left]);
cand_y.push_back(point_y[left]);
cand_x.push_back(point_x[right]);
cand_y.push_back(point_y[right]);
cand_x.push_back(point_x[lower]);
cand_y.push_back(point_y[lower]);
cand_x.push_back(point_x[upper]);
cand_y.push_back(point_y[upper]);
removeDuplicates(cand_x, cand_y);
}
}
void UFunczoneobst::removeDuplicates(vector<double> &cand_x, vector<double> &cand_y)
/**
* if two of the points are close to each other, use their centroid to replace them
* graph: left(0)<->lower(2)<->right(1)<->upper(3)<->left(0)
**/
{
double dist1 = 0;
double dist2 = 0;
// remove duplicates
// check left
dist1 = calcDist(cand_x[0], cand_y[0], cand_x[2], cand_y[2]);
dist2 = calcDist(cand_x[0], cand_y[0], cand_x[3], cand_y[3]);
cout<<dist1<<'\t'<<dist2<<endl;
if (dist1<=0.06)
{
// left and lower should be combined
vex_x.push_back((cand_x[0]+cand_x[2])/2);
vex_y.push_back((cand_y[0]+cand_y[2])/2);
vex_x.push_back(cand_x[1]);
vex_y.push_back(cand_y[1]);
vex_x.push_back(cand_x[3]);
vex_y.push_back(cand_y[3]);
}else if (dist2<=0.06)
{
// left and upper should be combined
vex_x.push_back((cand_x[0]+cand_x[3])/2);
vex_y.push_back((cand_y[0]+cand_y[3])/2);
vex_x.push_back(cand_x[1]);
vex_y.push_back(cand_y[1]);
vex_x.push_back(cand_x[2]);
vex_y.push_back(cand_y[2]);
}else
{
dist1 = calcDist(cand_x[1], cand_y[1], cand_x[2], cand_y[2]);
dist2 = calcDist(cand_x[1], cand_y[1], cand_x[3], cand_y[3]);
cout<<dist1<<'\t'<<dist2<<endl;
if (dist1<=0.06)
{
// right and lower should be combined
vex_x.push_back((cand_x[1]+cand_x[2])/2);
vex_y.push_back((cand_y[1]+cand_y[2])/2);
vex_x.push_back(cand_x[0]);
vex_y.push_back(cand_y[0]);
vex_x.push_back(cand_x[3]);
vex_y.push_back(cand_y[3]);
}else if (dist2<=0.06)
{
// right and upper should be combined
vex_x.push_back((cand_x[1]+cand_x[3])/2);
vex_y.push_back((cand_y[1]+cand_y[3])/2);
vex_x.push_back(cand_x[0]);
vex_y.push_back(cand_y[0]);
vex_x.push_back(cand_x[2]);
vex_y.push_back(cand_y[2]);
}else
{
vex_x.push_back(cand_x[0]);
vex_y.push_back(cand_y[0]);
vex_x.push_back(cand_x[1]);
vex_y.push_back(cand_y[1]);
vex_x.push_back(cand_x[2]);
vex_y.push_back(cand_y[2]);
vex_x.push_back(cand_x[3]);
vex_y.push_back(cand_y[3]);
}
}
}
double UFunczoneobst::calcDist(double x1, double y1, double x2, double y2)
/**
* calculate the distance between 2 points
**/
{
double dist = 0;
double dx = x2-x1;
double dy = y2-y1;
dist = sqrt(pow(dx,2)+pow(dy,2));
return dist;
}
void UFunczoneobst::sort(vector<double> &vec)
/**
* bubble sort, ascend order
**/
{
if (vec.size()>1)
{
for(int i=0;(double)i<vec.size()-1;i++)
{
for(int j=0;(double)j<vec.size()-1-i;j++)
{
if(vec[j]>vec[j+1])
{
double temp = vec[j];
vec[j] = vec[j+1];
vec[j+1] = temp;
}
}
}
}
}
double UFunczoneobst::calcAngle(int idx1, int idx2)
/**
* return rotating angle, range=(-180,180]
**/
{
double dx = vex_x[idx2] - vex_x[idx1];
double dy = vex_y[idx2] - vex_y[idx1];
double angle = atan2(dy,dx)*180/PI;
return angle;
}
\ No newline at end of file
/***************************************************************************
* 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
/////CONSTANTS
#define PI 3.1415926
/////
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <bits/stdc++.h>
#include <math.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <ulms4/ufunclaserbase.h>
using namespace std;
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
/**
* 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;
vector<double> point_x, point_y;
vector<double> vex_x,vex_y;
void scanit(ULaserData * data, vector<double> &x_laser, vector<double> &y_laser);
void transform(vector<double> &x_laser, vector<double> &y_laser, double pose[3]);
void getPoints();
void removeDuplicates(vector<double> &cand_x, vector<double> &cand_y);
double calcDist(double x1, double y1, double x2, double y2);
void sort(vector<double> &vec);
double calcAngle(int idx1, int idx2);
};
#endif
File added
This diff is collapsed.
main 0 → 100644
%% 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
......@@ -23,8 +23,8 @@ if ! [ ${PWD##*/} = "sim" ];then
fi
# initialize log files
if [ -f laserlogs.log ]; then
rm -f laserlogs.log
if [ -f detectResult.log ]; then
rm -f detectResult.log
fi
# add object
......
......@@ -77,9 +77,9 @@ gmth[14]=0
laser "calculatecost"
%% path for detecting the object
array "dox" 8
array "doy" 8
array "doth" 8
array "dox" 9
array "doy" 9
array "doth" 9
dox[1] = 0.5
doy[1] = 1.5
doth[1] = 0
......@@ -101,7 +101,7 @@ doth[6] = 3*pi/4
dox[7] = 2
doy[7] = 0.5
doth[7] = pi/2
dox[8] = 1
dox[8] = 1.01
doy[8] = 0.5
doth[8] = pi/4
......@@ -174,30 +174,42 @@ goto "top"
% function: subtask2
% recoggnize unknow objects
label "subtask2"
counter = 1
dist = 0.48-0.26
gcounter = 1
label "detectLoop"
xEnd = dox[counter]
yEnd = doy[counter]
thEnd = doth[counter]
xEnd = dox[gcounter]
yEnd = doy[gcounter]
thEnd = doth[gcounter]
eval gcounter
eval xEnd
eval yEnd
eval thEnd
wait 0.5
call "plan"
counter=counter+1
if (counter==4) "spPoint"
gcounter=gcounter+1
if (gcounter==4) "spPoint"
wait 1
laser "zoneobst logOn"
if (counter<9) "detectLoop"
call "update"
stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
laser "$string"
if (gcounter<9) "detectLoop"
laser "zoneobst recog"
return
label "spPoint"
fwd -0.32 @v 0.1
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
laser "zoneobst logOn"
call "update"
stringcat "zoneobst detect x="xCurr" y="yCurr" th="thCurr
laser "$string"
goto "detectLoop"
......
No preview for this file type
shape: Triangle center: [[2.68476934 1.56584345]] width: 0.3 height: 0.15 angle: 2.60124991477
\ No newline at end of file
shape: Rectangle center: [[2.31890252 1.35067841]] width: 0.4 height: 0.15 angle: 2.15942673599
\ No newline at end of file
......@@ -117,11 +117,9 @@ class Trian(MyObj):
def makePoints(self):
roi = (self.bound[0]+self.width, self.bound[1]+self.width, self.bound[2]-self.width, self.bound[3]-self.width)
# rotating angle
angle = random.uniform(0, math.pi)
angle = random.uniform(-math.pi, math.pi)
# center
o = np.array([[random.uniform(roi[0], roi[2])], [random.uniform(roi[1], roi[3])]])
with open('./spawnLog.log', 'a+') as f:
f.write('center: '+str(o.reshape((1,-1)))+'\twidth: '+str(self.width)+'\theight: '+str(self.height)+'\tangle: '+str(angle))
# right
p1 = self.rotate([self.width, 0], angle)+o
# upper
......@@ -151,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))
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')
if __name__ == "__main__":
spawn()
......
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment