add spawn objects

388box

-0.0	0.0	1.8	0
-0.0	1.8	1.8	1.8
-0.0 	0.0	0.0	1.8
-1.8	0.0	1.8	1.8

388line

- 0   1.0   2.3  1.0
-1.0  1.0   1.25 1.125
-1.25 1.125 1.5  1.375
-1.5 1.375  1.75 1.5
-1.75  1.5  4.25 1.5
-2.5   1.3  2.5  1.7
-3.0   1.3  3.0  1.7
-3.5   1.3  3.5  1.7
-4.0   1.3  4.0  1.7

388maze

-0.0	0.0	3.75	0.0	kuk
-0.75	0.75	1.5	0.75	kuk1
-2.25	0.75	3.75	0.75
-0.0	1.5	0.75	1.5
-1.5	1.5	3.0	1.5
-0.75	2.25	2.25	2.25
-0.0	3.0	1.5	3.0
-0.0	0.0	0.0	3.75	
-1.5	0.75	1.5	1.5	
-2.25	2.25	2.25	3.75
-3.0	1.5	3.0	3.0	
-3.75	0.0	3.75	3.75
-0.0	3.75	3.73	3.75

388project

@@ -15,4 +15,3 @@
 1.5	3.7	2.5	3.7	maze middle horizontal
 2.0	3.7	2.0	4.3	maze middle vertical
 0.9	4.3	3.1	4.3	maze top
-

388wall

-2.0 0.85 2.0 1.15

chindplugin/.deps/ufunczoneobst.d

+.deps/ufunczoneobst.d ufunczoneobst.o: ufunczoneobst.cpp ufunczoneobst.h

chindplugin/Makefile

+# 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))

chindplugin/ufunczoneobst.cpp

+/***************************************************************************
+ *   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.");
+}

chindplugin/ufunczoneobst.h

+/***************************************************************************
+ *   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
+

formerSrc/data.debug

-Debug file for the robot initialization
----------------------------------------
-Wed Apr 15 11:44:27 2020
----------------------------------------
-
-Module : robotinfo
-   type      differential     enum number        0
-   name      smr
-
-Module : odometry
-   cl           0.000102
-   cr           0.000102
-   w            0.260000
-   robotlength  0.265000
-   ts           0.010000
-   maxtick          1000
-   control             0
-   enctype             0
-
-Module : motion control
-   ts           0.010000
-   line_gain    0.050000
-   line_tau     10.000000
-   line_alfa    0.300000
-   wall_gain    1.000000
-   wall_tau     0.700000
-   wall_alfa    0.200000
-   gain         1.000000
-   tau          0.160000
-   alfa         0.200000
-   drive_kangle 2.250000
-   drive_kdist  4.800000
-   w            0.260000
-   robotlength  0.265000
-   lim          0.200000
-   stopdist     0.180000
-   alarmdist    0.100000
-   velcmd       0.300000
-   acccmd       0.300000
-   nolinedist   0.200000
-
-Module : motor control
-   velscalel    100.000000
-   velscaler    100.000000
-   kp           66.000000
-   ki           10.000000
-
-Module : line sensor
-   size                8
-   k_filt       0.750000
-
-Module : filter
-   measurementnoise_std_x           0.100000
-   measurementnoise_std_y           0.110000
-   processnoise_std_stearingangle   0.010000
-   processnoise_std_dist            0.020000
-   type                  EKF
-   run                          1
-   use                          0
-   size_input                   2
-   size_state                   3
-   size_noise_process           2
-   size_output                  2
-   size_noise_output            2
-   gpsoffset_easting     0.000000
-   gpsoffset_northing    0.000000
-
-Module : gpsmouse
-   hostname     localhost
-   port             9500
-   SBAS                0
-   run                 1
-   use                 1
-

formerSrc/ex11

-laser "addline startx=0 starty=0 endx=1.8 endy=0 name='line1'"
-laser "addline startx=1.8 starty=0 endx=1.8 endy=1.8 name='line2'"
-laser "addline startx=1.8 starty=1.8 endx=0 endy=1.8 name='line3'"
-laser "addline startx=0 starty=1.8 endx=0 endy=0 name='line4'"
-laser "setinitpose x=0.95 y=1 th=0"
-laser "setinitcov Cx=1 Cy=1 Cth=1"
-laser "localize"
-invtrans $l0 $l1 $l2 $odox $odoy $odoth

formerSrc/ex11-1

-laser "localize"
-wait 1
-wait 1
-invtrans $l0 $l1 $l2 $odox $odoy $odoth
-wait 2
-eval $l0
-eval $l1
-eval $l2
-eval $res0
-eval $res1
-eval $res2

formerSrc/ex11-2

-% since drive is based on odometry
-% the start point is (0,0,0), in simulator it is set (0.9, 0.9, 0)
-% go to the first corner
-ignoreobstacles
-fwd 0.5
-ignoreobstacles
-turn 90
-ignoreobstacles
-fwd 0.5
-ignoreobstacles
-turn 90
-% N is the counter
-% Now we arrive the first point
-N=0
-label "loop"
-drive -0.5 0.5 180: ($targetdist <0.25)
-wait 1
-drive -0.5 -0.5 270: ($targetdist <0.25)
-wait 1
-drive 0.5 -0.5 0: ($targetdist <0.25)
-wait 1
-drive 0.5 0.5 90: ($targetdist <0.25)
-wait 1
-N = N+1
-wait 1
-if(N<5) "loop"

formerSrc/ex11-3

-% drive the robot via Kalman Filter
-laser "localize"
-laser “push t='1.0' cmd='localize' “
-wait 1
-N=0
-ignoreobstacles
-fwd 0.5
-wait 1
-label "loop"
-drivew 1.4 1.4 90 :($targetdist<0.25)
-wait 3
-drivew 0.4 1.4 180 :($targetdist<0.25)
-wait 3
-drivew 0.4 0.4 270 :($targetdist<0.25)
-wait 3
-drivew 1.4 0.4 0 :($targetdist<0.25)
-wait 3
-N = N+1
-if (N<9) "loop"

formerSrc/ex11-4

-% drive the robot via Kalman Filter 
-laser "resetplanner"
-set "$odox" 0
-set "$odoy" 0
-set "$odoth" 0
-laser "localize"
-laser “push t='1.0' cmd='localize'“
-wait 3
-yh = 1.4
-yl = 1.2
-ignoreobstacles
-fwd 0.5
-wait 1
-drivew 1.4 yh 90:($targetdist<0.25)
-wait 3
-label "loop"
-call "move"
-yh = yh-0.4
-yl = yl-0.4
-wait 1
-if (yl<0.2) "stop"
-goto "loop"
-label "move"
-drivew 0.4 yh 180:($targetdist<0.25)
-wait 3
-turn 90
-fwd 0.2
-wait 3
-%drivew 0.4 yl 270:($targetdist<0.25)
-%wait 3
-drivew 1.4 yl 0:($targetdist<0.25)
-wait 3
-turn -90
-fwd 0.2
-turn 180
-wait 3
-return
-label "show"
-invtrans $l0 $l1 $l2 $odox $odoy $odoth
-wait 1
-eval $res0
-eval $res1
-eval $res2
-return
-label "stop"
\ No newline at end of file

formerSrc/ex12

-drive -0.5 0.5 135:($targetdist<0.25)
-drive -1 1 135:($targetdist<0.25)
-drive -1.5 1.5 135:($targetdist<0.25)
-drive -2 1.5 180:($targetdist<0.25)
-drive -2.5 1.5 180:($targetdist<0.25)
-drive -3 2 135:($targetdist<0.25)
-drive -2.5 3 45:($targetdist<0.25)
-ignoreobstacles
-drive -2 3.5 45:($targetdist<0.25)
-ignoreobstacles
-drive -1.5 3.5 0:($targetdist<0.25)
-drive -1 3.5 0:($targetdist<0.25)
-drive -0.5 4 45:($targetdist<0.25)s
-drive -1 5 135:($targetdist<0.25)
-ignoreobstacles
-drive -1.5 5.5 135:($targetdist<0.25)
-drive -2 5.5 180:($targetdist<0.25)
-drive -2.5 5.5 180:($targetdist<0.25)
-drive -3 5.5 180:($targetdist<0.25)
-drive -3.5 5.5 180:($targetdist<0.25)
-drive -4 5.5 180
-stop
\ No newline at end of file

formerSrc/ex13-1

-laser "findroute startx=3.2 starty=0.3 endx=0.3 endy=3.3"
-wait 1
-c = $l4
-eval c
-label "loop"
-stringcat "getpoint p="c
-laser "$string"
-wait 1
-eval $l5
-eval $l6
-eval $l7
-c=c-1
-if (c>-1) "loop"
\ No newline at end of file