diff --git a/cpp_src/Makefile b/cpp_src/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..3ab682c8da0103a160c1f6e0ba9f7628b74a79e3
--- /dev/null
+++ b/cpp_src/Makefile
@@ -0,0 +1,72 @@
+# 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))
diff --git a/cpp_src/makeit.sh b/cpp_src/makeit.sh
new file mode 100644
index 0000000000000000000000000000000000000000..13969dd915848241456046b0b7447352173ae3b8
--- /dev/null
+++ b/cpp_src/makeit.sh
@@ -0,0 +1,5 @@
+#!/usr/bin/bash
+cd ~/home/smr/mobotware/aurs-plugins/auzoneobst/
+make clean
+make
+cp objectDetect.so.0 /home/smr/sim/
diff --git a/cpp_src/ufunczoneobst.cpp b/cpp_src/ufunczoneobst.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f3eafc13c4c4a6c914add7b9b4cc9d0bb16ddca
--- /dev/null
+++ b/cpp_src/ufunczoneobst.cpp
@@ -0,0 +1,577 @@
+/***************************************************************************
+ * 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
diff --git a/cpp_src/ufunczoneobst.h b/cpp_src/ufunczoneobst.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd231e504a3dd769816e0283975a1a060b676735
--- /dev/null
+++ b/cpp_src/ufunczoneobst.h
@@ -0,0 +1,81 @@
+/***************************************************************************
+ * 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
+
+
diff --git a/cpp_src/ufunczoneobst.o b/cpp_src/ufunczoneobst.o
new file mode 100644
index 0000000000000000000000000000000000000000..ca57ef6ad962cef5620ebd4f3410d0c1ae3f1754
Binary files /dev/null and b/cpp_src/ufunczoneobst.o differ
diff --git a/laserlogs.log b/laserlogs.log
deleted file mode 100644
index 07ac06a6a1a162f22d7d00677f80477f575d59fc..0000000000000000000000000000000000000000
--- a/laserlogs.log
+++ /dev/null
@@ -1,2896 +0,0 @@
-0 3.532
-1 3.532
-2 3.532
-3 3.533
-4 3.534
-5 3.535
-6 3.536
-7 3.538
-8 2.466
-9 2.175
-10 1.946
-11 1.76
-12 1.64
-13 1.642
-14 1.644
-15 1.645
-16 1.647
-17 1.649
-18 1.651
-19 1.654
-20 1.656
-21 1.659
-22 1.661
-23 1.664
-24 1.667
-25 1.67
-26 1.673
-27 1.677
-28 1.68
-29 1.684
-30 1.688
-31 1.692
-32 1.696
-33 1.7
-34 1.704
-35 1.709
-36 1.714
-37 1.718
-38 1.724
-39 1.729
-40 1.734
-41 1.74
-42 1.745
-43 1.751
-44 1.757
-45 1.763
-46 1.77
-47 1.776
-48 1.783
-49 1.79
-50 1.797
-51 1.804
-52 1.812
-53 1.82
-54 1.828
-55 1.836
-56 1.844
-57 1.853
-58 1.861
-59 1.87
-60 1.88
-61 1.889
-62 1.879
-63 1.852
-64 1.826
-65 1.801
-66 1.776
-67 1.752
-68 1.729
-69 1.707
-70 1.686
-71 1.665
-72 1.644
-73 1.625
-74 1.606
-75 1.587
-76 1.569
-77 1.552
-78 1.535
-79 1.518
-80 1.502
-81 1.487
-82 1.472
-83 1.457
-84 1.443
-85 1.429
-86 1.415
-87 3.446
-88 2.257
-89 2.237
-90 2.217
-91 2.198
-92 2.179
-93 2.161
-94 2.143
-95 2.125
-96 2.108
-97 2.092
-98 2.076
-99 2.06
-100 2.045
-101 2.03
-102 2.015
-103 2.001
-104 1.987
-105 1.974
-106 1.961
-107 1.948
-108 1.935
-109 1.923
-110 1.911
-111 1.899
-112 1.888
-113 1.877
-114 3.899
-115 3.877
-116 3.856
-117 3.835
-118 3.814
-119 3.794
-120 3.775
-121 3.756
-122 3.737
-123 3.719
-124 3.702
-125 0.001
-126 0.001
-127 0.001
-128 0.001
-129 0.001
-130 0.001
-131 0.001
-132 0.001
-133 0.001
-134 0.001
-135 0.001
-136 0.001
-137 0.001
-138 0.001
-139 0.001
-140 0.001
-141 0.001
-142 0.001
-143 0.001
-144 0.001
-145 0.001
-146 0.001
-147 0.001
-148 0.001
-149 0.001
-150 0.001
-151 0.001
-152 0.001
-153 0.001
-154 0.001
-155 0.001
-156 0.001
-157 0.001
-158 0.001
-159 0.001
-160 0.001
-161 0.001
-162 0.001
-163 0.001
-164 1.712
-165 1.719
-166 1.726
-167 1.733
-168 1.741
-169 1.749
-170 1.756
-171 1.764
-172 1.773
-173 1.781
-174 1.79
-175 1.799
-176 1.808
-177 1.817
-178 1.827
-179 1.837
-180 1.847
-181 1.858
-182 1.868
-183 3.261
-184 3.262
-185 3.263
-186 3.264
-187 3.265
-188 3.267
-189 3.269
-190 3.271
-191 3.274
-192 3.277
-193 3.28
-194 3.283
-195 3.286
-196 3.29
-197 3.294
-198 3.298
-199 3.303
-200 3.308
-201 3.313
-202 3.318
-203 3.324
-204 3.33
-205 3.336
-206 3.342
-207 3.349
-208 3.356
-209 3.363
-210 3.371
-211 3.379
-212 3.387
-213 3.396
-214 3.404
-215 3.413
-216 3.423
-217 3.433
-218 3.443
-219 3.453
-220 3.464
-221 3.475
-222 3.486
-223 3.498
-224 3.51
-225 3.522
-226 3.535
-227 3.548
-228 3.561
-229 3.574
-230 3.506
-231 3.441
-232 3.379
-233 3.319
-234 3.261
-235 3.206
-236 3.152
-237 3.101
-238 3.052
-239 3.004
-240 2.958
-241 2.913
-242 2.871
-243 2.829
-244 2.789
-245 2.75
-246 2.713
-247 2.677
-248 2.642
-249 2.608
-250 2.575
-251 2.543
-252 2.512
-253 2.482
-254 2.453
-255 2.424
-256 2.397
-257 2.37
-258 2.345
-259 2.319
-260 2.295
-261 2.271
-262 2.248
-263 2.226
-264 2.204
-265 2.182
-266 2.162
-267 2.142
-268 2.122
-269 2.103
-270 2.084
-271 0.001
-272 0.001
-273 0.001
-274 0.001
-275 0.001
-276 0.001
-277 0.001
-278 0.001
-279 0.001
-280 0.001
-281 0.001
-282 0.001
-283 0.001
-284 0.001
-285 0.001
-286 0.001
-287 0.001
-288 0.001
-289 1.808
-290 1.797
-291 1.786
-292 1.775
-293 1.765
-294 1.754
-295 1.744
-296 1.735
-297 1.725
-298 1.716
-299 1.707
-300 1.698
-301 1.69
-302 1.682
-303 1.673
-304 1.665
-305 1.658
-306 1.65
-307 1.643
-308 1.636
-309 1.629
-310 1.622
-311 1.615
-312 1.609
-313 1.603
-314 1.597
-315 1.591
-316 1.585
-317 1.579
-318 1.574
-319 1.569
-320 1.564
-321 1.559
-322 1.554
-323 1.549
-324 1.545
-325 1.54
-326 1.536
-327 1.532
-328 1.528
-329 1.524
-330 1.52
-331 1.517
-332 1.514
-333 1.51
-334 1.507
-335 1.504
-336 1.501
-337 1.498
-338 1.496
-339 1.493
-340 1.491
-341 1.488
-342 1.486
-343 1.484
-344 1.482
-345 1.48
-346 1.479
-347 1.477
-348 1.476
-349 1.474
-350 1.473
-351 1.472
-352 1.471
-353 1.47
-354 1.469
-355 1.469
-356 1.468
-357 1.468
-358 1.467
-359 1.467
-360 1.467
-
-0 0.801
-1 0.794
-2 0.787
-3 0.78
-4 0.774
-5 0.768
-6 0.761
-7 0.755
-8 0.75
-9 0.744
-10 0.738
-11 0.733
-12 0.728
-13 0.723
-14 0.718
-15 0.713
-16 0.708
-17 0.703
-18 0.699
-19 0.694
-20 0.69
-21 0.686
-22 0.682
-23 0.678
-24 0.674
-25 0.67
-26 0.666
-27 0.663
-28 0.659
-29 0.656
-30 0.652
-31 0.649
-32 0.646
-33 0.643
-34 0.64
-35 0.637
-36 0.634
-37 0.631
-38 0.628
-39 0.626
-40 0.623
-41 0.621
-42 0.618
-43 0.616
-44 0.613
-45 0.611
-46 0.609
-47 0.607
-48 0.605
-49 0.603
-50 0.601
-51 0.599
-52 0.597
-53 1.229
-54 1.225
-55 1.222
-56 1.218
-57 1.215
-58 1.212
-59 1.209
-60 1.206
-61 1.203
-62 1.2
-63 1.198
-64 1.195
-65 1.193
-66 1.191
-67 1.188
-68 1.186
-69 1.184
-70 1.182
-71 1.181
-72 0.001
-73 0.001
-74 0.001
-75 0.001
-76 0.001
-77 0.001
-78 0.001
-79 0.001
-80 0.001
-81 0.001
-82 0.001
-83 0.001
-84 0.001
-85 0.001
-86 0.001
-87 0.001
-88 0.001
-89 0.001
-90 0.001
-91 0.001
-92 0.001
-93 0.001
-94 0.001
-95 0.001
-96 0.001
-97 0.001
-98 0.001
-99 0.001
-100 0.001
-101 0.001
-102 0.001
-103 0.001
-104 0.001
-105 0.001
-106 0.001
-107 0.001
-108 0.001
-109 0.001
-110 0.001
-111 2.91
-112 2.915
-113 2.92
-114 2.925
-115 2.93
-116 2.936
-117 2.942
-118 2.948
-119 2.955
-120 2.961
-121 2.968
-122 2.975
-123 2.983
-124 2.991
-125 2.999
-126 3.007
-127 3.015
-128 3.024
-129 3.033
-130 3.043
-131 3.052
-132 3.062
-133 3.073
-134 3.083
-135 3.094
-136 3.105
-137 3.117
-138 3.129
-139 3.141
-140 1.434
-141 1.454
-142 1.474
-143 1.496
-144 1.518
-145 1.541
-146 1.564
-147 1.589
-148 1.614
-149 1.641
-150 1.668
-151 1.697
-152 3.332
-153 3.35
-154 3.368
-155 3.386
-156 3.405
-157 3.424
-158 3.444
-159 3.465
-160 3.486
-161 3.507
-162 3.529
-163 3.551
-164 3.574
-165 3.598
-166 3.622
-167 3.647
-168 3.672
-169 3.646
-170 3.608
-171 3.57
-172 3.534
-173 3.499
-174 3.464
-175 3.431
-176 3.399
-177 3.367
-178 3.336
-179 3.306
-180 3.277
-181 3.248
-182 3.221
-183 3.194
-184 3.167
-185 3.142
-186 3.117
-187 3.092
-188 3.068
-189 3.045
-190 3.023
-191 3.001
-192 2.979
-193 2.958
-194 2.938
-195 2.918
-196 2.898
-197 2.879
-198 2.861
-199 2.843
-200 2.825
-201 2.808
-202 2.791
-203 2.775
-204 2.759
-205 2.743
-206 2.728
-207 2.713
-208 2.698
-209 2.684
-210 2.671
-211 2.657
-212 2.644
-213 2.631
-214 2.619
-215 2.607
-216 2.595
-217 2.583
-218 2.572
-219 2.561
-220 2.551
-221 0.001
-222 0.001
-223 0.001
-224 0.001
-225 0.001
-226 0.001
-227 0.001
-228 0.001
-229 0.001
-230 0.001
-231 0.001
-232 0.001
-233 0.001
-234 0.001
-235 0.001
-236 0.001
-237 0.001
-238 0.001
-239 2.397
-240 2.391
-241 2.386
-242 2.38
-243 2.375
-244 2.37
-245 2.365
-246 2.361
-247 2.356
-248 2.352
-249 2.348
-250 2.344
-251 2.341
-252 2.337
-253 2.334
-254 2.331
-255 2.328
-256 2.326
-257 2.323
-258 2.321
-259 2.319
-260 2.317
-261 2.315
-262 2.313
-263 2.312
-264 2.311
-265 2.31
-266 2.309
-267 2.308
-268 2.308
-269 2.307
-270 2.307
-271 2.307
-272 2.307
-273 2.308
-274 2.308
-275 2.309
-276 2.31
-277 2.311
-278 2.312
-279 2.313
-280 2.315
-281 2.317
-282 2.319
-283 2.321
-284 2.323
-285 2.326
-286 2.328
-287 2.331
-288 2.334
-289 2.338
-290 2.341
-291 2.345
-292 2.348
-293 2.352
-294 2.357
-295 2.361
-296 2.365
-297 2.37
-298 2.375
-299 2.38
-300 2.386
-301 2.391
-302 2.397
-303 2.403
-304 2.409
-305 2.416
-306 2.423
-307 2.429
-308 2.436
-309 2.444
-310 2.451
-311 2.459
-312 2.467
-313 2.476
-314 2.484
-315 2.493
-316 2.502
-317 2.511
-318 2.521
-319 2.531
-320 2.541
-321 2.551
-322 2.562
-323 2.567
-324 2.523
-325 2.48
-326 2.439
-327 2.399
-328 2.361
-329 2.324
-330 2.288
-331 2.254
-332 2.221
-333 2.189
-334 2.158
-335 2.128
-336 2.099
-337 2.071
-338 2.044
-339 2.018
-340 1.992
-341 1.967
-342 1.943
-343 1.92
-344 1.898
-345 1.876
-346 1.855
-347 1.834
-348 1.814
-349 1.795
-350 1.776
-351 1.757
-352 1.739
-353 1.722
-354 1.705
-355 1.689
-356 1.673
-357 1.657
-358 1.642
-359 1.627
-360 1.613
-
-0 0.001
-1 0.001
-2 0.001
-3 0.001
-4 0.001
-5 0.001
-6 0.001
-7 0.001
-8 0.001
-9 0.001
-10 0.001
-11 0.001
-12 0.001
-13 0.001
-14 0.001
-15 0.001
-16 0.001
-17 0.001
-18 0.001
-19 0.001
-20 0.001
-21 0.001
-22 2.08
-23 2.084
-24 2.087
-25 2.091
-26 2.095
-27 2.099
-28 2.104
-29 2.108
-30 2.113
-31 2.118
-32 2.123
-33 2.128
-34 2.134
-35 2.14
-36 2.145
-37 2.152
-38 2.158
-39 2.164
-40 2.171
-41 2.178
-42 2.185
-43 2.192
-44 2.2
-45 2.207
-46 2.215
-47 2.224
-48 2.232
-49 2.241
-50 2.25
-51 2.259
-52 2.268
-53 2.278
-54 2.288
-55 2.298
-56 2.308
-57 2.319
-58 2.33
-59 2.341
-60 2.353
-61 2.365
-62 2.377
-63 2.389
-64 2.402
-65 2.415
-66 2.428
-67 2.442
-68 2.456
-69 2.471
-70 2.486
-71 2.501
-72 2.516
-73 2.532
-74 2.549
-75 2.565
-76 2.583
-77 2.6
-78 2.618
-79 2.637
-80 0.93
-81 0.879
-82 0.834
-83 0.794
-84 0.757
-85 0.724
-86 0.693
-87 0.665
-88 0.665
-89 0.69
-90 0.717
-91 0.747
-92 0.778
-93 0.813
-94 0.851
-95 0.892
-96 0.939
-97 2.91
-98 2.888
-99 2.866
-100 2.845
-101 2.824
-102 2.804
-103 2.784
-104 2.765
-105 2.746
-106 2.727
-107 2.709
-108 2.692
-109 2.675
-110 2.658
-111 2.642
-112 2.626
-113 2.611
-114 2.596
-115 2.581
-116 2.566
-117 2.552
-118 2.539
-119 2.525
-120 2.512
-121 2.5
-122 2.487
-123 2.475
-124 2.464
-125 2.452
-126 2.441
-127 2.43
-128 2.42
-129 2.409
-130 2.399
-131 2.389
-132 2.38
-133 2.371
-134 2.362
-135 2.353
-136 2.344
-137 2.336
-138 2.328
-139 2.32
-140 2.313
-141 2.305
-142 2.298
-143 2.291
-144 2.284
-145 2.278
-146 2.272
-147 2.266
-148 2.26
-149 2.254
-150 2.249
-151 2.243
-152 2.238
-153 2.233
-154 2.229
-155 2.224
-156 2.22
-157 2.216
-158 2.212
-159 2.208
-160 2.204
-161 2.201
-162 2.198
-163 2.195
-164 2.192
-165 2.189
-166 2.186
-167 2.184
-168 0.001
-169 0.001
-170 0.001
-171 0.001
-172 0.001
-173 0.001
-174 0.001
-175 0.001
-176 0.001
-177 0.001
-178 0.001
-179 0.001
-180 0.001
-181 0.001
-182 0.001
-183 0.001
-184 0.001
-185 0.001
-186 0.001
-187 0.001
-188 0.001
-189 2.174
-190 2.176
-191 2.177
-192 2.179
-193 2.181
-194 2.183
-195 2.186
-196 2.188
-197 2.191
-198 2.194
-199 2.197
-200 2.2
-201 2.203
-202 2.207
-203 2.21
-204 2.214
-205 2.218
-206 2.223
-207 2.227
-208 2.232
-209 2.237
-210 2.242
-211 2.247
-212 2.252
-213 2.258
-214 2.264
-215 2.27
-216 2.276
-217 2.282
-218 2.289
-219 2.296
-220 2.303
-221 2.31
-222 2.318
-223 2.325
-224 2.333
-225 2.342
-226 2.35
-227 2.359
-228 2.368
-229 2.377
-230 2.386
-231 2.396
-232 2.406
-233 2.416
-234 2.427
-235 2.437
-236 2.449
-237 2.46
-238 2.472
-239 2.483
-240 2.496
-241 2.508
-242 2.521
-243 2.534
-244 2.548
-245 2.562
-246 2.576
-247 2.591
-248 2.606
-249 2.621
-250 2.637
-251 2.653
-252 2.669
-253 2.686
-254 2.704
-255 2.721
-256 2.74
-257 2.758
-258 2.777
-259 2.797
-260 2.817
-261 2.838
-262 2.859
-263 2.881
-264 2.903
-265 2.912
-266 2.884
-267 2.857
-268 2.831
-269 2.806
-270 2.781
-271 2.757
-272 2.733
-273 2.71
-274 2.688
-275 2.666
-276 2.644
-277 2.624
-278 2.603
-279 2.584
-280 2.564
-281 2.546
-282 2.527
-283 2.509
-284 2.492
-285 2.475
-286 2.459
-287 2.442
-288 2.427
-289 2.411
-290 2.396
-291 2.382
-292 2.367
-293 2.353
-294 2.34
-295 2.326
-296 2.314
-297 2.301
-298 2.289
-299 2.277
-300 2.265
-301 2.253
-302 2.242
-303 2.231
-304 2.221
-305 2.211
-306 2.2
-307 2.191
-308 2.181
-309 2.172
-310 2.163
-311 2.154
-312 2.145
-313 2.137
-314 2.129
-315 2.121
-316 2.113
-317 2.106
-318 2.099
-319 2.092
-320 2.085
-321 2.078
-322 2.072
-323 2.065
-324 2.059
-325 2.053
-326 2.048
-327 2.042
-328 2.037
-329 2.032
-330 2.027
-331 2.022
-332 2.018
-333 2.013
-334 2.009
-335 2.005
-336 2.001
-337 1.997
-338 1.994
-339 1.99
-340 0.001
-341 0.001
-342 0.001
-343 0.001
-344 0.001
-345 0.001
-346 0.001
-347 0.001
-348 0.001
-349 0.001
-350 0.001
-351 0.001
-352 0.001
-353 0.001
-354 0.001
-355 0.001
-356 0.001
-357 0.001
-358 0.001
-359 0.001
-360 0.001
-
-0 1.73
-1 1.745
-2 1.761
-3 1.777
-4 1.793
-5 1.81
-6 1.828
-7 1.845
-8 1.864
-9 1.883
-10 1.902
-11 1.922
-12 1.942
-13 1.963
-14 1.985
-15 2.007
-16 2.03
-17 2.054
-18 2.079
-19 2.104
-20 2.13
-21 2.156
-22 2.184
-23 2.212
-24 2.242
-25 2.272
-26 2.304
-27 2.336
-28 2.37
-29 2.404
-30 2.44
-31 2.477
-32 2.516
-33 2.556
-34 2.597
-35 2.616
-36 2.604
-37 2.592
-38 2.581
-39 2.57
-40 2.56
-41 2.549
-42 2.539
-43 2.529
-44 2.52
-45 2.51
-46 2.501
-47 2.493
-48 2.484
-49 2.476
-50 2.468
-51 2.46
-52 2.452
-53 2.445
-54 2.438
-55 2.431
-56 2.424
-57 2.418
-58 2.412
-59 2.406
-60 2.4
-61 2.394
-62 2.389
-63 2.384
-64 2.379
-65 2.374
-66 2.369
-67 2.365
-68 2.361
-69 2.357
-70 2.353
-71 2.349
-72 2.346
-73 2.343
-74 2.34
-75 2.337
-76 2.334
-77 2.332
-78 2.329
-79 2.327
-80 2.325
-81 2.323
-82 2.322
-83 2.32
-84 2.319
-85 2.318
-86 2.317
-87 2.317
-88 2.316
-89 2.316
-90 2.316
-91 2.316
-92 2.316
-93 2.316
-94 2.317
-95 2.317
-96 2.318
-97 2.319
-98 2.321
-99 2.322
-100 2.324
-101 2.325
-102 2.327
-103 2.329
-104 2.332
-105 0.749
-106 0.747
-107 0.744
-108 0.741
-109 0.739
-110 0.736
-111 0.734
-112 0.731
-113 0.729
-114 0.727
-115 0.724
-116 0.722
-117 0.72
-118 0.718
-119 0.716
-120 0.715
-121 0.713
-122 0.711
-123 0.709
-124 0.708
-125 0.706
-126 0.705
-127 0.703
-128 0.702
-129 0.701
-130 0.7
-131 0.698
-132 0.697
-133 0.696
-134 0.695
-135 0.694
-136 0.693
-137 0.692
-138 0.692
-139 0.691
-140 0.69
-141 0.69
-142 0.689
-143 0.689
-144 0.688
-145 0.688
-146 0.687
-147 0.687
-148 0.687
-149 0.687
-150 0.686
-151 2.681
-152 2.695
-153 2.709
-154 2.724
-155 2.739
-156 2.754
-157 2.77
-158 2.786
-159 2.802
-160 2.819
-161 2.836
-162 2.854
-163 2.872
-164 2.891
-165 2.91
-166 2.93
-167 2.95
-168 2.97
-169 2.991
-170 3.013
-171 3.035
-172 3.058
-173 3.081
-174 3.105
-175 3.13
-176 3.155
-177 3.181
-178 3.207
-179 3.234
-180 3.262
-181 3.291
-182 3.32
-183 3.35
-184 3.381
-185 3.413
-186 3.446
-187 3.479
-188 3.514
-189 3.55
-190 3.586
-191 3.603
-192 3.577
-193 3.552
-194 3.527
-195 3.503
-196 3.48
-197 3.457
-198 3.435
-199 3.413
-200 3.392
-201 3.371
-202 3.351
-203 3.332
-204 3.312
-205 3.294
-206 3.276
-207 3.258
-208 3.24
-209 3.223
-210 3.207
-211 3.191
-212 3.175
-213 3.16
-214 3.145
-215 3.13
-216 3.116
-217 3.102
-218 3.089
-219 3.076
-220 3.063
-221 3.051
-222 3.039
-223 3.027
-224 3.015
-225 3.004
-226 2.993
-227 2.983
-228 2.973
-229 2.963
-230 2.953
-231 2.944
-232 2.935
-233 2.926
-234 2.917
-235 2.909
-236 2.901
-237 2.893
-238 2.886
-239 2.879
-240 2.872
-241 2.865
-242 2.859
-243 2.852
-244 2.846
-245 2.841
-246 2.835
-247 2.83
-248 2.825
-249 2.82
-250 0.001
-251 0.001
-252 0.001
-253 0.001
-254 0.001
-255 0.001
-256 0.001
-257 0.001
-258 0.001
-259 0.001
-260 0.001
-261 0.001
-262 0.001
-263 0.001
-264 0.001
-265 0.001
-266 0.001
-267 0.001
-268 0.001
-269 0.001
-270 0.001
-271 0.001
-272 0.001
-273 0.001
-274 0.001
-275 0.001
-276 0.001
-277 0.001
-278 0.001
-279 0.001
-280 0.001
-281 0.001
-282 0.001
-283 0.001
-284 0.001
-285 0.001
-286 0.001
-287 0.001
-288 0.001
-289 0.001
-290 1.087
-291 1.088
-292 1.09
-293 1.092
-294 1.094
-295 1.096
-296 1.098
-297 1.1
-298 1.103
-299 1.105
-300 1.108
-301 1.11
-302 1.113
-303 1.116
-304 1.119
-305 1.122
-306 1.125
-307 1.128
-308 1.131
-309 1.135
-310 1.138
-311 1.142
-312 1.145
-313 1.149
-314 0.507
-315 0.509
-316 0.511
-317 0.513
-318 0.515
-319 0.517
-320 0.519
-321 0.521
-322 0.523
-323 0.525
-324 0.528
-325 0.53
-326 0.532
-327 0.535
-328 0.537
-329 0.54
-330 0.543
-331 0.545
-332 0.548
-333 0.551
-334 0.554
-335 0.557
-336 0.56
-337 0.563
-338 0.567
-339 0.57
-340 0.573
-341 0.577
-342 0.581
-343 0.584
-344 0.588
-345 0.592
-346 0.596
-347 0.6
-348 0.604
-349 0.609
-350 0.613
-351 0.617
-352 0.622
-353 0.627
-354 0.632
-355 0.637
-356 0.642
-357 0.647
-358 0.652
-359 0.658
-360 0.664
-
-0 1.543
-1 1.543
-2 1.543
-3 1.544
-4 1.544
-5 1.544
-6 1.545
-7 1.546
-8 1.547
-9 1.547
-10 1.549
-11 1.55
-12 1.551
-13 1.552
-14 1.554
-15 1.556
-16 1.557
-17 1.559
-18 1.561
-19 1.564
-20 1.566
-21 1.568
-22 1.571
-23 1.573
-24 1.576
-25 1.579
-26 1.582
-27 1.585
-28 1.589
-29 1.592
-30 1.596
-31 1.599
-32 1.603
-33 1.607
-34 1.611
-35 1.616
-36 1.62
-37 1.625
-38 1.63
-39 1.634
-40 1.64
-41 1.645
-42 1.65
-43 1.656
-44 1.661
-45 1.667
-46 1.673
-47 1.679
-48 1.686
-49 1.692
-50 1.699
-51 1.706
-52 1.713
-53 1.72
-54 1.728
-55 1.736
-56 1.744
-57 1.752
-58 1.76
-59 1.769
-60 1.777
-61 1.786
-62 1.795
-63 1.805
-64 1.815
-65 1.824
-66 1.835
-67 1.845
-68 0.001
-69 0.001
-70 0.001
-71 0.001
-72 0.001
-73 0.001
-74 0.001
-75 0.001
-76 0.001
-77 0.001
-78 0.001
-79 0.001
-80 0.001
-81 0.001
-82 0.001
-83 0.001
-84 0.001
-85 0.001
-86 0.001
-87 2.118
-88 2.136
-89 2.154
-90 2.173
-91 2.192
-92 2.211
-93 2.231
-94 2.252
-95 2.273
-96 2.295
-97 2.317
-98 2.34
-99 2.364
-100 2.388
-101 2.413
-102 2.438
-103 2.465
-104 2.492
-105 2.52
-106 2.549
-107 2.579
-108 2.609
-109 2.641
-110 2.673
-111 2.707
-112 2.741
-113 2.777
-114 2.814
-115 2.852
-116 2.891
-117 2.932
-118 2.974
-119 3.017
-120 3.062
-121 3.109
-122 3.157
-123 3.207
-124 3.259
-125 3.313
-126 3.369
-127 3.427
-128 3.488
-129 3.551
-130 3.57
-131 3.555
-132 3.541
-133 3.527
-134 3.514
-135 3.501
-136 3.488
-137 3.476
-138 3.464
-139 3.452
-140 3.441
-141 3.43
-142 3.42
-143 3.409
-144 3.399
-145 3.39
-146 3.38
-147 3.371
-148 3.363
-149 3.354
-150 3.346
-151 3.338
-152 3.331
-153 3.324
-154 3.317
-155 3.31
-156 3.303
-157 3.297
-158 3.291
-159 3.286
-160 3.281
-161 3.276
-162 0.625
-163 0.62
-164 0.616
-165 0.612
-166 0.609
-167 0.605
-168 0.601
-169 0.598
-170 0.594
-171 0.591
-172 0.587
-173 0.584
-174 0.581
-175 0.578
-176 0.575
-177 0.572
-178 0.569
-179 0.566
-180 0.564
-181 0.561
-182 0.558
-183 0.556
-184 0.553
-185 0.551
-186 0.549
-187 0.558
-188 0.567
-189 0.577
-190 0.587
-191 0.598
-192 0.609
-193 0.62
-194 0.632
-195 0.645
-196 0.658
-197 0.672
-198 0.686
-199 0.701
-200 0.717
-201 0.733
-202 0.75
-203 0.769
-204 0.788
-205 0.808
-206 0.001
-207 0.001
-208 0.001
-209 0.001
-210 0.001
-211 0.001
-212 0.001
-213 0.001
-214 0.001
-215 0.001
-216 0.001
-217 0.001
-218 0.001
-219 0.001
-220 0.001
-221 0.001
-222 0.001
-223 0.001
-224 0.001
-225 0.001
-226 0.001
-227 0.001
-228 0.001
-229 0.001
-230 0.001
-231 0.001
-232 0.001
-233 0.001
-234 0.001
-235 3.63
-236 3.647
-237 3.664
-238 3.681
-239 3.699
-240 3.717
-241 3.736
-242 3.755
-243 3.775
-244 3.795
-245 1.806
-246 1.817
-247 1.827
-248 1.837
-249 1.848
-250 1.859
-251 1.871
-252 1.882
-253 1.894
-254 1.907
-255 1.919
-256 1.932
-257 1.945
-258 1.959
-259 1.973
-260 1.987
-261 2.002
-262 2.017
-263 2.032
-264 2.048
-265 2.064
-266 2.08
-267 2.097
-268 2.115
-269 2.133
-270 2.151
-271 2.17
-272 3.335
-273 1.342
-274 1.354
-275 1.367
-276 1.38
-277 1.393
-278 1.407
-279 1.421
-280 1.436
-281 1.451
-282 1.466
-283 1.482
-284 1.499
-285 1.516
-286 1.533
-287 1.551
-288 1.569
-289 1.588
-290 1.608
-291 1.628
-292 1.649
-293 1.67
-294 1.692
-295 1.715
-296 1.739
-297 1.763
-298 1.788
-299 1.811
-300 1.801
-301 1.792
-302 1.783
-303 1.775
-304 1.766
-305 1.758
-306 1.75
-307 1.743
-308 1.735
-309 1.728
-310 1.72
-311 1.713
-312 1.707
-313 1.7
-314 1.694
-315 1.687
-316 1.681
-317 1.675
-318 1.669
-319 1.664
-320 1.658
-321 1.653
-322 1.648
-323 1.643
-324 1.638
-325 1.634
-326 1.629
-327 1.625
-328 1.621
-329 1.617
-330 1.613
-331 1.609
-332 1.605
-333 1.602
-334 1.598
-335 1.595
-336 1.592
-337 1.589
-338 1.586
-339 1.584
-340 1.581
-341 1.579
-342 1.576
-343 1.574
-344 1.572
-345 1.57
-346 1.568
-347 1.567
-348 1.565
-349 1.564
-350 1.562
-351 1.561
-352 1.727
-353 1.955
-354 2.254
-355 2.661
-356 3.458
-357 3.457
-358 3.457
-359 3.456
-360 3.456
-
-0 0.001
-1 0.001
-2 0.001
-3 0.001
-4 0.001
-5 0.001
-6 0.001
-7 0.001
-8 0.001
-9 0.001
-10 0.001
-11 0.001
-12 0.001
-13 0.001
-14 0.001
-15 0.001
-16 0.001
-17 0.001
-18 0.001
-19 0.001
-20 0.001
-21 0.001
-22 0.001
-23 1.257
-24 1.274
-25 1.291
-26 1.309
-27 1.327
-28 1.346
-29 1.366
-30 1.386
-31 1.408
-32 1.429
-33 1.452
-34 1.476
-35 1.5
-36 1.525
-37 1.552
-38 1.579
-39 1.608
-40 1.637
-41 1.668
-42 1.701
-43 1.734
-44 1.77
-45 1.806
-46 1.845
-47 1.885
-48 1.927
-49 1.972
-50 2.018
-51 2.067
-52 2.119
-53 2.173
-54 2.231
-55 2.292
-56 2.356
-57 2.424
-58 2.497
-59 2.574
-60 2.656
-61 2.744
-62 2.838
-63 2.926
-64 2.92
-65 2.914
-66 2.909
-67 2.903
-68 2.898
-69 2.893
-70 2.889
-71 2.884
-72 2.88
-73 2.876
-74 2.872
-75 2.869
-76 2.866
-77 2.862
-78 2.86
-79 2.857
-80 2.855
-81 2.852
-82 2.85
-83 2.849
-84 2.847
-85 2.846
-86 2.845
-87 2.844
-88 2.843
-89 2.843
-90 2.843
-91 2.843
-92 2.843
-93 2.843
-94 2.844
-95 2.845
-96 2.846
-97 2.847
-98 2.849
-99 2.85
-100 2.852
-101 2.855
-102 2.857
-103 2.86
-104 2.863
-105 2.866
-106 2.869
-107 2.872
-108 2.876
-109 2.88
-110 2.884
-111 2.889
-112 2.893
-113 2.898
-114 2.904
-115 2.909
-116 2.915
-117 2.92
-118 2.927
-119 2.933
-120 2.94
-121 2.946
-122 2.954
-123 2.961
-124 2.969
-125 2.977
-126 2.985
-127 2.993
-128 3.002
-129 3.011
-130 3.02
-131 3.03
-132 3.04
-133 0.001
-134 0.001
-135 0.001
-136 0.001
-137 0.001
-138 0.001
-139 0.001
-140 0.001
-141 0.001
-142 0.001
-143 0.001
-144 0.001
-145 0.001
-146 0.001
-147 0.001
-148 0.001
-149 0.001
-150 0.001
-151 0.001
-152 0.001
-153 0.001
-154 0.001
-155 0.001
-156 0.001
-157 0.001
-158 0.001
-159 0.001
-160 0.001
-161 0.001
-162 0.001
-163 0.001
-164 0.001
-165 0.001
-166 0.001
-167 0.001
-168 0.001
-169 0.001
-170 0.001
-171 0.001
-172 0.001
-173 0.001
-174 3.811
-175 3.841
-176 3.872
-177 3.903
-178 3.936
-179 3.969
-180 0.001
-181 0.001
-182 0.001
-183 0.001
-184 0.001
-185 0.001
-186 0.001
-187 0.001
-188 0.001
-189 0.001
-190 0.001
-191 0.001
-192 0.001
-193 3.078
-194 3.057
-195 3.036
-196 3.016
-197 2.996
-198 2.977
-199 2.958
-200 2.94
-201 2.922
-202 2.905
-203 2.887
-204 2.871
-205 2.855
-206 2.142
-207 2.172
-208 2.203
-209 2.236
-210 2.269
-211 2.303
-212 2.339
-213 2.376
-214 2.415
-215 2.455
-216 2.496
-217 2.54
-218 2.584
-219 2.631
-220 3.981
-221 3.965
-222 3.949
-223 3.278
-224 3.266
-225 3.254
-226 3.242
-227 1.074
-228 1.028
-229 0.985
-230 0.947
-231 0.911
-232 0.878
-233 0.847
-234 0.818
-235 0.791
-236 0.779
-237 0.786
-238 0.793
-239 0.8
-240 0.808
-241 0.815
-242 0.823
-243 0.831
-244 0.839
-245 0.847
-246 0.856
-247 0.865
-248 0.874
-249 0.883
-250 2.44
-251 2.436
-252 2.432
-253 2.429
-254 2.426
-255 2.423
-256 2.42
-257 2.418
-258 2.415
-259 2.413
-260 2.411
-261 2.409
-262 2.407
-263 2.406
-264 2.405
-265 2.404
-266 2.403
-267 2.402
-268 2.401
-269 2.401
-270 2.401
-271 2.401
-272 2.401
-273 2.401
-274 2.402
-275 2.403
-276 2.404
-277 2.405
-278 2.406
-279 2.408
-280 2.409
-281 2.411
-282 2.413
-283 0.001
-284 0.001
-285 0.001
-286 0.001
-287 0.001
-288 0.001
-289 0.001
-290 0.001
-291 0.001
-292 0.001
-293 0.001
-294 0.001
-295 0.001
-296 0.001
-297 0.001
-298 0.001
-299 0.001
-300 0.001
-301 0.001
-302 0.001
-303 0.001
-304 0.001
-305 3.9
-306 3.793
-307 3.693
-308 3.598
-309 3.508
-310 3.422
-311 3.341
-312 3.264
-313 3.191
-314 3.121
-315 3.054
-316 2.991
-317 2.93
-318 2.872
-319 2.816
-320 2.762
-321 0.001
-322 0.001
-323 0.001
-324 0.001
-325 0.001
-326 0.001
-327 0.001
-328 0.001
-329 0.001
-330 0.001
-331 0.001
-332 0.001
-333 0.001
-334 0.001
-335 0.001
-336 0.001
-337 0.001
-338 0.001
-339 0.001
-340 0.001
-341 0.001
-342 0.001
-343 0.001
-344 0.001
-345 0.001
-346 0.001
-347 0.001
-348 0.001
-349 0.001
-350 0.001
-351 0.001
-352 0.001
-353 0.001
-354 0.001
-355 0.001
-356 0.001
-357 0.001
-358 0.001
-359 0.001
-360 0.001
-
-0 2.026
-1 2.026
-2 2.026
-3 2.027
-4 2.027
-5 2.028
-6 2.029
-7 2.029
-8 2.031
-9 2.032
-10 2.033
-11 2.035
-12 2.036
-13 2.038
-14 2.04
-15 2.042
-16 2.045
-17 2.047
-18 2.05
-19 2.053
-20 2.056
-21 2.059
-22 2.062
-23 2.066
-24 2.069
-25 2.073
-26 2.077
-27 2.081
-28 2.086
-29 2.09
-30 2.095
-31 2.1
-32 2.105
-33 2.11
-34 2.116
-35 2.121
-36 2.127
-37 2.133
-38 2.14
-39 2.146
-40 2.153
-41 2.159
-42 2.167
-43 2.174
-44 2.181
-45 2.189
-46 2.197
-47 2.205
-48 2.213
-49 2.222
-50 2.231
-51 2.24
-52 2.249
-53 2.259
-54 2.269
-55 0.001
-56 0.001
-57 0.001
-58 0.001
-59 0.001
-60 0.001
-61 0.001
-62 0.001
-63 0.001
-64 0.001
-65 0.001
-66 0.001
-67 0.001
-68 0.001
-69 0.001
-70 0.001
-71 0.001
-72 0.001
-73 0.001
-74 0.001
-75 0.001
-76 0.001
-77 0.001
-78 0.001
-79 0.001
-80 0.001
-81 0.001
-82 0.001
-83 0.001
-84 0.001
-85 0.001
-86 0.001
-87 0.001
-88 0.001
-89 0.001
-90 0.001
-91 0.001
-92 0.001
-93 0.001
-94 0.001
-95 0.001
-96 0.001
-97 0.001
-98 0.001
-99 0.001
-100 0.001
-101 0.001
-102 3.201
-103 3.236
-104 3.272
-105 3.309
-106 3.346
-107 3.385
-108 3.425
-109 3.467
-110 3.509
-111 3.553
-112 3.599
-113 3.646
-114 3.694
-115 3.744
-116 3.796
-117 3.849
-118 3.904
-119 3.961
-120 0.001
-121 0.001
-122 0.001
-123 0.001
-124 0.001
-125 0.001
-126 0.001
-127 0.001
-128 0.001
-129 0.001
-130 2.584
-131 2.573
-132 2.563
-133 2.553
-134 2.543
-135 2.534
-136 2.525
-137 2.516
-138 2.507
-139 2.499
-140 2.491
-141 2.483
-142 2.475
-143 2.468
-144 2.46
-145 2.453
-146 2.447
-147 2.44
-148 2.434
-149 2.428
-150 2.422
-151 2.416
-152 2.411
-153 2.406
-154 2.4
-155 2.396
-156 2.391
-157 2.387
-158 2.382
-159 2.378
-160 1.832
-161 1.925
-162 2.028
-163 2.143
-164 2.271
-165 2.964
-166 2.96
-167 2.957
-168 2.954
-169 2.951
-170 2.949
-171 2.947
-172 2.945
-173 2.943
-174 2.941
-175 2.94
-176 2.939
-177 2.938
-178 2.937
-179 2.937
-180 2.936
-181 2.936
-182 2.937
-183 2.937
-184 2.938
-185 2.939
-186 2.94
-187 2.941
-188 2.943
-189 2.944
-190 2.946
-191 2.949
-192 2.951
-193 2.954
-194 2.331
-195 2.171
-196 2.031
-197 1.908
-198 1.8
-199 2.367
-200 2.37
-201 2.374
-202 2.378
-203 2.382
-204 2.386
-205 2.391
-206 2.395
-207 2.4
-208 2.405
-209 2.41
-210 2.416
-211 2.421
-212 2.427
-213 2.433
-214 2.44
-215 2.446
-216 2.453
-217 2.46
-218 2.467
-219 2.474
-220 2.482
-221 2.49
-222 2.498
-223 2.507
-224 2.515
-225 2.524
-226 2.533
-227 1.033
-228 1.027
-229 1.021
-230 1.016
-231 1.01
-232 1.005
-233 0.999
-234 0.994
-235 0.989
-236 0.984
-237 0.979
-238 0.975
-239 0.97
-240 0.966
-241 0.961
-242 0.957
-243 0.953
-244 0.949
-245 0.945
-246 0.941
-247 0.937
-248 0.934
-249 0.93
-250 0.927
-251 0.924
-252 0.92
-253 0.917
-254 0.914
-255 0.911
-256 0.908
-257 0.906
-258 0.903
-259 0.9
-260 0.898
-261 0.895
-262 0.893
-263 0.001
-264 0.001
-265 0.001
-266 0.001
-267 0.001
-268 0.001
-269 0.001
-270 0.001
-271 0.001
-272 0.001
-273 0.001
-274 0.001
-275 0.001
-276 0.001
-277 0.001
-278 0.001
-279 0.001
-280 0.001
-281 0.001
-282 0.001
-283 0.001
-284 0.001
-285 0.001
-286 0.001
-287 0.001
-288 0.001
-289 0.001
-290 0.001
-291 0.001
-292 0.001
-293 0.001
-294 0.001
-295 0.001
-296 0.001
-297 0.001
-298 0.001
-299 0.001
-300 0.001
-301 0.001
-302 0.001
-303 0.001
-304 0.001
-305 0.001
-306 2.219
-307 2.21
-308 2.2
-309 2.191
-310 2.182
-311 2.173
-312 2.164
-313 2.156
-314 2.147
-315 2.14
-316 2.132
-317 2.124
-318 2.117
-319 2.11
-320 2.103
-321 2.096
-322 2.09
-323 2.084
-324 2.078
-325 2.072
-326 2.066
-327 2.06
-328 2.055
-329 2.05
-330 2.045
-331 2.04
-332 2.036
-333 2.031
-334 2.027
-335 2.023
-336 2.019
-337 2.015
-338 2.012
-339 2.008
-340 2.005
-341 2.002
-342 1.999
-343 1.996
-344 1.993
-345 1.991
-346 1.989
-347 1.987
-348 1.985
-349 1.983
-350 1.981
-351 1.98
-352 1.978
-353 1.977
-354 1.976
-355 1.975
-356 1.974
-357 1.974
-358 1.973
-359 1.973
-360 1.973
-
-0 0.001
-1 0.001
-2 0.001
-3 0.001
-4 0.001
-5 0.001
-6 3.415
-7 2.934
-8 2.643
-9 2.645
-10 2.647
-11 2.649
-12 2.651
-13 2.654
-14 2.657
-15 2.66
-16 2.663
-17 2.666
-18 2.67
-19 2.674
-20 2.678
-21 2.682
-22 2.686
-23 2.691
-24 2.696
-25 2.701
-26 2.706
-27 2.712
-28 2.718
-29 2.724
-30 2.73
-31 2.737
-32 2.743
-33 2.75
-34 2.758
-35 2.765
-36 2.773
-37 2.781
-38 2.789
-39 2.798
-40 2.807
-41 2.796
-42 2.733
-43 2.672
-44 2.614
-45 2.559
-46 2.507
-47 2.456
-48 2.408
-49 2.362
-50 2.318
-51 2.275
-52 3.602
-53 3.618
-54 3.634
-55 3.65
-56 3.667
-57 3.684
-58 0.001
-59 0.001
-60 0.001
-61 0.001
-62 3.066
-63 3.022
-64 2.98
-65 2.939
-66 2.9
-67 2.861
-68 2.824
-69 2.788
-70 2.754
-71 2.72
-72 2.687
-73 2.655
-74 2.625
-75 2.595
-76 3.348
-77 3.371
-78 3.395
-79 3.419
-80 3.444
-81 3.47
-82 3.496
-83 3.523
-84 3.55
-85 0.001
-86 0.001
-87 0.001
-88 0.001
-89 0.001
-90 0.001
-91 0.001
-92 0.001
-93 0.001
-94 0.001
-95 0.001
-96 0.001
-97 0.001
-98 0.001
-99 0.001
-100 0.001
-101 0.001
-102 0.001
-103 0.001
-104 0.001
-105 0.001
-106 0.001
-107 0.001
-108 2.122
-109 2.121
-110 2.12
-111 2.12
-112 2.119
-113 2.119
-114 2.119
-115 2.119
-116 2.119
-117 2.119
-118 2.12
-119 2.121
-120 2.121
-121 2.122
-122 2.123
-123 2.125
-124 2.126
-125 2.128
-126 0.001
-127 0.001
-128 0.001
-129 0.001
-130 0.001
-131 0.001
-132 0.001
-133 0.001
-134 0.001
-135 0.001
-136 3.537
-137 3.525
-138 3.513
-139 3.502
-140 3.491
-141 3.48
-142 3.469
-143 3.459
-144 3.449
-145 3.439
-146 3.43
-147 3.421
-148 3.412
-149 3.404
-150 3.396
-151 3.388
-152 3.381
-153 3.374
-154 3.367
-155 3.36
-156 3.354
-157 3.348
-158 3.342
-159 3.336
-160 3.331
-161 3.326
-162 3.321
-163 3.317
-164 3.313
-165 3.309
-166 3.305
-167 3.302
-168 3.299
-169 3.296
-170 3.293
-171 3.291
-172 3.289
-173 3.287
-174 3.285
-175 3.284
-176 3.283
-177 3.282
-178 3.281
-179 3.281
-180 3.281
-181 3.281
-182 3.281
-183 3.282
-184 3.283
-185 3.284
-186 3.286
-187 3.287
-188 3.289
-189 3.291
-190 3.294
-191 3.296
-192 3.299
-193 3.302
-194 3.306
-195 3.31
-196 3.308
-197 3.116
-198 2.945
-199 2.792
-200 2.654
-201 2.53
-202 2.416
-203 2.313
-204 2.218
-205 2.131
-206 2.051
-207 1.976
-208 1.907
-209 1.843
-210 1.783
-211 1.727
-212 1.675
-213 1.625
-214 1.579
-215 0.001
-216 0.001
-217 0.001
-218 0.001
-219 0.001
-220 0.001
-221 0.001
-222 0.001
-223 0.001
-224 0.001
-225 0.001
-226 0.001
-227 1.159
-228 1.136
-229 1.114
-230 1.093
-231 1.073
-232 1.054
-233 1.036
-234 1.018
-235 1.001
-236 0.984
-237 0.968
-238 0.953
-239 0.938
-240 0.924
-241 0.911
-242 0.897
-243 0.884
-244 0.872
-245 0.86
-246 0.849
-247 0.837
-248 0.827
-249 0.816
-250 0.806
-251 0.796
-252 0.786
-253 0.777
-254 0.768
-255 0.759
-256 0.751
-257 0.743
-258 0.735
-259 0.727
-260 0.719
-261 0.712
-262 0.705
-263 0.698
-264 0.691
-265 0.684
-266 0.678
-267 0.672
-268 0.665
-269 0.66
-270 0.654
-271 0.648
-272 0.643
-273 0.637
-274 0.632
-275 0.627
-276 0.622
-277 0.617
-278 0.613
-279 0.608
-280 0.604
-281 0.599
-282 0.595
-283 0.591
-284 0.587
-285 0.583
-286 0.579
-287 0.575
-288 0.571
-289 0.568
-290 0.564
-291 0.561
-292 0.558
-293 0.554
-294 0.551
-295 0.548
-296 0.545
-297 0.542
-298 0.539
-299 0.537
-300 0.534
-301 0.531
-302 0.529
-303 0.526
-304 0.524
-305 0.521
-306 0.519
-307 0.517
-308 0.514
-309 0.512
-310 0.51
-311 0.508
-312 0.506
-313 0.504
-314 0.502
-315 0.5
-316 0.499
-317 0.497
-318 0.495
-319 0.494
-320 0.492
-321 0.491
-322 0.489
-323 0.488
-324 0.486
-325 0.485
-326 0.484
-327 0.482
-328 0.481
-329 0.48
-330 0.479
-331 0.478
-332 0.477
-333 0.476
-334 0.475
-335 0.474
-336 0.473
-337 0.472
-338 0.471
-339 0.47
-340 0.47
-341 0.469
-342 0.468
-343 0.468
-344 0.467
-345 0.466
-346 0.466
-347 0.465
-348 0.465
-349 0.465
-350 0.464
-351 0.464
-352 0.464
-353 0.463
-354 0.463
-355 0.463
-356 0.463
-357 0.463
-358 0.463
-359 0.463
-360 0.462
-
diff --git a/main b/main
new file mode 100644
index 0000000000000000000000000000000000000000..3c979930d3922e0444d607a7661ba832ffc20f33
--- /dev/null
+++ b/main
@@ -0,0 +1,346 @@
+%% 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
diff --git a/main.sh b/main.sh
index 2afadcdcab00da37c488ef09301aa4e7c9819fd2..191f36e9aa7733a6939d1afa359465a31fd5e386 100644
--- a/main.sh
+++ b/main.sh
@@ -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
diff --git a/mrc_src/main b/mrc_src/main
index 29736c70fee3d7e3ae7b881e9e7e9dae6ab10047..20bad8faeb657aef69bd7b9403669f18f2adf2b8 100644
--- a/mrc_src/main
+++ b/mrc_src/main
@@ -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"
diff --git a/objectDetect.so.0 b/objectDetect.so.0
index 29e6e3d32e4aee191380d538298bf7c045c3c6f9..1fdb22fd49c559a5a7706aaade5247eb9b2bad69 100644
Binary files a/objectDetect.so.0 and b/objectDetect.so.0 differ
diff --git a/spawnLog.log b/spawnLog.log
index dcd6a4392d602759a0a936054b417172ad915edc..13b6ef237ded9b3e122b6226230b8b3d89a33b71 100644
--- a/spawnLog.log
+++ b/spawnLog.log
@@ -1 +1 @@
-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
diff --git a/spawnObjs.py b/spawnObjs.py
index ee13b7b7e09bf4c8f4780a55cd2bba98bbc03f03..56078ecf5152dc9105219a68fa20edb146c0bf52 100644
--- a/spawnObjs.py
+++ b/spawnObjs.py
@@ -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()
diff --git a/splitLog.py b/splitLog.py
new file mode 100644
index 0000000000000000000000000000000000000000..a392972abda7ac16a9d22786e5da454b954e1335
--- /dev/null
+++ b/splitLog.py
@@ -0,0 +1,18 @@
+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