add spawn objects

formerSrc/test9

-drive @v 0.2 :($irdistfrontleft<0.45)|($irdistfrontmiddle<0.45)|($irdistfrontright<0.45)
-stop
-wait 3
-switch ($fiducialid)
-turn 180 @v 0.2
-case 1 
-turn 180 @v 0.2
-stop
-fwd 1
-turn -180 @v 0.2
-followline "bl" @v 0.2 :($crossingblackline==1)
-stop
-fwd 0.25 @v 0.2
-stop
-turn 90 @v 0.2
-%followline "bm" @v 0.1 :($drivendist>0.1)
-drive @v 0.2 :($drivendist>0.1)
-stop
-case 2 
-turn 180 @v 0.2
-stop
-fwd 1
-turn -180 @v 0.2
-followline "bl" @v 0.2 :($crossingblackline==1)
-stop
-fwd 0.25 @v 0.2
-stop
-turn -90 @v 0.2
-%followline "bm" @v 0.1 :($drivendist>0.1)
-drive @v 0.2 :($drivendist>0.1)
-stop
-endswitch
-

formerSrc/text8

-drive @v 0.3 :($irdistfrontleft<0.4)|($irdistfrontmiddle<0.4)|($irdistfrontright<0.4)
-stop

formerSrc/try1_zbst

-laser "scanpush cmd ='zone0bst'"
-wait 2
-
-log "$l0" "l0" 
-wait 1
-log "$l1" "l1" 
-wait 1
-log "$l2" "l2" 
-wait 1
-log "$l3" "l3" 
-wait 1
-log "$l4" "l4" 
-wait 1
-log "$l5" "l5"
-wait 1
-log "$l6" "l6"
-wait 1
-log "$l7" "l7"
-wait 1
-"$l8" "l8"
-wait 1
-%stop

formerSrc/ulmsserver.ini

-
-server imagepath="./"
-server datapath="./"
-server replayPath="./log"
-
-#Setup server for port 20100+N where N is team nr.
-server port="24919"
-
-#Load basic modules
-module load="odoPose"
-module load="laserPool"
-module load="v360"
-# module load for odometry control and global varable access
-module load=var
-module load=mappose
-
-# live laser scanner on SMR
-#scanset devtype=urg devname="/dev/ttyACM0"
-#scanset def=urg
-#scanset mirror=true
-
-##Simulated scanner (port: 19100+N where N is team nr)
-scanset devType=sim devName=localhost:19001
-scanset def=sim
-
-#Set scanner position with respect of SMR center
-scanset x=0.255 z=0.04
-scanset width=180
-scanset mirror="true"
-scanset logOpen
-scanset log=used
-
-####################################################
-##  Load modules and enter setup commands below   ##
-####################################################
-module load="aupoly.so.0"
-module load="aulocalize.so.0"
-addline startx=0.0 starty=0.0 endx=1.8 endy=0.0 name="bottom"
-addline startx=1.8 starty=0.0 endx=1.8 endy=1.8 name="right"
-addline startx=0.0 starty=0.0 endx=0.0 endy=1.8 name="left"
-addline startx=0.0 starty=1.8 endx=1.8 endy=1.8 name="top"
-setinitpose x=0.90 y=0.90 th=0
-setinitcov Cx=0.04 Cy=0.04 Cth=0.04
\ No newline at end of file

formerSrc/ulmsserver13.ini

-
-server imagepath="./"
-server datapath="./"
-server replayPath="./log"
-
-#Setup server for port 20100+N where N is team nr.
-server port="24919"
-
-#Load basic modules
-module load="odoPose"
-module load="laserPool"
-module load="v360"
-# module load for odometry control and global varable access
-module load=var
-module load=mappose
-
-# live laser scanner on SMR
-#scanset devtype=urg devname="/dev/ttyACM0"
-#scanset def=urg
-#scanset mirror=true
-
-##Simulated scanner (port: 19100+N where N is team nr)
-scanset devType=sim devName=localhost:19001
-scanset def=sim
-
-#Set scanner position with respect of SMR center
-scanset x=0.255 z=0.04
-scanset width=180
-scanset mirror="true"
-scanset logOpen
-scanset log=used
-
-####################################################
-##  Load modules and enter setup commands below   ##
-####################################################
-module load="aupoly.so.0"
-module load="aulocalize.so.0"
-module load="auplan.so.0"
-%module load="auzoneobst.so.0"
-setInitPose x=1.7 y=0.3 th=0
-setInitCov Cx=0.04 Cy=0.04 Cth=0.04
-resetplanner
-addpoint pno=1 x=0.2 y=0.3
-addpoint pno=2 x=1.7 y=0.3
-addpoint pno=3 x=3.2 y=0.3
-addpoint pno=4 x=0.2 y=1.05
-addpoint pno=5 x=0.95 y=1.05
-addpoint pno=6 x=1.7 y=1.05
-addpoint pno=7 x=3.2 y=1.05
-addpoint pno=8 x=0.2 y=1.8
-addpoint pno=9 x=0.95 y=1.8
-addpoint pno=10 x=2.45 y=1.8
-addpoint pno=11 x=0.2 y=2.55
-addpoint pno=12 x=1.7 y=2.55
-addpoint pno=13 x=0.2 y=3.3
-addpoint pno=14 x=1.7 y=3.3
-addpoint pno=15 x=2.45 y=3.3
-addpoint pno=16 x=3.2 y=3.3
-addcon pno1=1 pno2=2
-addcon pno1=2 pno2=1
-addcon pno1=2 pno2=3
-addcon pno1=3 pno2=2
-addcon pno1=1 pno2=4
-addcon pno1=4 pno2=1
-addcon pno1=2 pno2=6
-addcon pno1=6 pno2=2
-addcon pno1=4 pno2=5
-addcon pno1=5 pno2=4
-addcon pno1=6 pno2=7
-addcon pno1=7 pno2=6
-addcon pno1=5 pno2=9
-addcon pno1=9 pno2=5
-addcon pno1=7 pno2=16
-addcon pno1=16 pno2=7
-addcon pno1=8 pno2=9
-addcon pno1=9 pno2=8
-addcon pno1=9 pno2=10
-addcon pno1=10 pno2=9
-addcon pno1=8 pno2=11
-addcon pno1=11 pno2=8
-addcon pno1=10 pno2=15
-addcon pno1=15 pno2=10
-addcon pno1=11 pno2=12
-addcon pno1=12 pno2=11
-addcon pno1=12 pno2=14
-addcon pno1=14 pno2=12
-addcon pno1=13 pno2=14
-addcon pno1=14 pno2=13
-addcon pno1=15 pno2=16
-addcon pno1=16 pno2=15
-calculatecost
\ No newline at end of file

main.sh

 #!usr/bin/bash
 # install gnome-terminal
+if ! [ -x "$(command -v pip)" ]; then
+sudo apt install python-pip
+sleep 1
+pip install numpy
+pip install PyYAML
+sleep 1
+fi
 if ! [ -x "$(command -v gnome-terminal)" ]; then
+  sudo apt-get update
   sudo apt-get install gnome-terminal
   sleep 1
 fi
+# terminal 0: add object
+python spawnObjs.py
+sleep 1
 # terminal 1: laser server
 gnome-terminal -t "laser server" -x bash -c "cd /home/smr/sim/bash_src/;
 sh laser.sh;exec bash;"
@@ -21,7 +32,7 @@ gnome-terminal -t "mrc" -x bash -c "cd /home/smr/sim/bash_src/;
 sh mov.sh;exec bash;"
 sleep 1
 # terminate
-## print processes
+## show processes
 echo "processes"
 ps -ef | grep "bash" | grep -v grep | awk '{print $2}'
 ps
@@ -37,6 +48,17 @@ echo $flag
         then
 	  ## clean logg files
           rm -r *.logg
+	  ## restore config files
+          if [ -f 388project_temp ]; then
+	  rm -r 388project
+          mv 388project_temp 388project
+	  fi
+	  sleep 1
+          cd mrc_src
+	  if [ -f createWalls_temp ];then
+          rm -r createWalls
+          mv createWalls_temp createWalls
+          fi
           ## kill all terminal processes
 	  gnome-terminal -t "close client" -x killall -9 qclient
           gnome-terminal -t "clear all" -x killall -9 bash  

maze

--0.25 -0.14 5.25 -0.14
-5.25 -0.14 5.25 5.64
-5.25 5.64 -0.25 5.64
--0.25 5.64 -0.25 -0.14
--0.25 3.64 3.25 3.64
-1.75 2.14 5.25 2.14

mrc_src/data.debug

 Debug file for the robot initialization
 ---------------------------------------
-Mon May 25 16:45:39 2020
+Tue May 26 19:14:40 2020
 ---------------------------------------
 
 Module : robotinfo

objConfig.yaml

+# RANDOM: bool ,if spawn objects randomly
+RANDOM: True
+# INDEX: 1-4, assign the index of the object, only works when RANDOM=False
+# index 1: Rectangle, width=0.4, height=0.15 
+# index 2: Rectangle, width=0.3, height=0.2
+# index 3: Triangle, width=0.4, height=0.1
+# index 4: Triangle, width=0.3, height=0.15
+INDEX: 1
\ No newline at end of file

spawnLog.log

+shape: Triangle	center: [[2.57248992 1.74175571]]	width: 0.3	height: 0.2	angle: 2.11407176003
\ No newline at end of file

spawnObjs.py

+#! /usr/bin/python
+# -*- coding: UTF-8 -*-
+'''
+author: group 16
+date: 05/26/2020
+'''
+import random
+import numpy as np
+from abc import ABCMeta, abstractmethod
+import math
+import os
+import shutil
+import yaml
+
+###---CONSTANTS---###
+BOUND = (1,1,3,2)
+RECT_NUMBER = 2
+RECT_HEIGHT = [0.15, 0.2]
+RECT_WIDTH = [0.4, 0.3]
+TRIAN_NUMBER = 2
+TRIAN_HEIGHT = [0.1, 0.15]
+TRIAN_WIDTH = [0.4, 0.3]
+PROJ_NAME = "./388project"
+MRC_NAME = "./mrc_src/createWalls"
+###---------------###
+
+## read configuration
+def read_config(config_path):
+    with open(config_path, 'r') as f:
+        cfg = yaml.load(f.read(), Loader=yaml.FullLoader)
+    return cfg or {}
+
+# initialize logfile
+if os.path.exists('./spawnLog.log'):
+    os.remove('./spawnLog.log')
+
+# create temp files for revision
+shutil.copy(PROJ_NAME, PROJ_NAME+'_temp')
+shutil.copy(MRC_NAME, MRC_NAME+'_temp')
+
+# create objects
+class MyObj(object):
+    '''
+    object
+    '''
+    def __init__(self, bound, width, height):
+        super(MyObj,self).__init__()
+        self.bound = bound
+        self.width = width
+        self.height = height
+        self.origin = []
+        self._points = []
+        self.makePoints()
+        self.writeText()
+
+    @abstractmethod
+    def makePoints(self):
+        pass
+
+    @property
+    def points(self):
+        return self._points
+
+    @staticmethod
+    def rotate(point, angle):
+        return np.array([[math.cos(angle), -math.sin(angle)],[math.sin(angle), math.cos(angle)]]).dot(np.array([[point[0]],[point[1]]]))
+    
+    def writeText(self):
+        '''
+        generate the text to write
+        '''
+        text = '\n'
+        for i in range(len(self._points)-1):
+            text+= str(round(self._points[i][0], 2))+'\t'+str(round(self._points[i][1], 2))+'\t'+str(round(self._points[i+1][0], 2))+'\t'+str(round(self._points[i+1][1], 2))+'\n'
+        with open(PROJ_NAME,'a+') as f:
+            f.write(text)
+        text = '\n'
+        for i in range(len(self._points)-1):
+            text+= "laser\t\"addline\tstartx="+str(round(self._points[i][0], 2))+'\tstarty='+str(round(self._points[i][1], 2))+'\tendx='+str(round(self._points[i+1][0], 2))+'\tendy='+str(round(self._points[i+1][1], 2))+'\tname=\''+str(random.random())+'\'\"\n'
+        with open(MRC_NAME,'a+') as f:
+            f.write(text)
+
+    
+
+
+class Rect(MyObj):
+    '''
+    Rectangle
+    ''' 
+    def __init__(self, bound, width, height):
+        super(Rect,self).__init__(bound, width, height)
+
+    def makePoints(self):
+        # diagonal of the rectangle
+        ax = np.linalg.norm([self.width, self.height], ord=2)/2
+        roi = (self.bound[0]+ax, self.bound[1]+ax, self.bound[2]-ax, self.bound[3]-ax)
+        # rotating angle
+        angle = random.uniform(0, math.pi)
+        # center
+        o = np.array([[random.uniform(roi[0], roi[2])], [random.uniform(roi[1], roi[3])]])
+        # left-lower
+        p1 = self.rotate([-self.width/2, -self.height/2], angle)+o
+        # right-lower
+        p2 = self.rotate([self.width/2, -self.height/2], angle)+o
+        # right-upper
+        p3 = self.rotate([self.width/2, self.height/2], angle)+o
+        # left-upper
+        p4 = self.rotate([-self.width/2, self.height/2], angle)+o
+        self._points = [p1,p2,p3,p4,p1]
+        self.angle = angle
+        self.origin = o
+        
+
+class Trian(MyObj):
+    '''
+    Triangle
+    '''
+    def __init__(self, bound, width, height):
+        super(Trian, self).__init__(bound, width, height)
+
+    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)
+        # 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
+        p2 = self.rotate([0, self.height], angle)+o
+        self._points = [o,p1,p2,o]
+        self.angle = angle
+        self.origin = o
+
+def spawn():
+    # read confiugrations
+    cfg = read_config('./objConfig.yaml')
+    RANDOM = cfg['RANDOM']
+    INDEX = cfg['INDEX']
+    shape_bin = ['Rectangle','Triangle']
+    if RANDOM:
+        # shape of the object
+        shape = random.randint(0,1)
+        # size of the object
+        idx = random.randint(0,1)
+    else:
+        shape = INDEX//3
+        idx = 1-INDEX%2
+    if shape:
+        # rectangle
+        obj = Rect(BOUND, RECT_WIDTH[idx], RECT_HEIGHT[idx])
+    else:
+        obj = Trian(BOUND, TRIAN_WIDTH[idx], TRIAN_HEIGHT[idx])
+    # write log files
+    with open('./spawnLog.log', 'a+') 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))
+
+if __name__ == "__main__":
+    spawn()
+
+
+
+
+ 
+
+
+
+

test

-turn 90
\ No newline at end of file

ucamserver.ini

-# camera server scriptfile
-server imagepath="."
-server datapath="."
-server replayPath="foo"
-# server port=24920
-#
-module load=var
-module load=gmk
-var gmk.diagonal=0
-module load=odopose
-module load=mappose
-module load=utmpose
-module load="aukinect.so.0"
-module load="aucamrectify.so.0"
-module load="austereo.so.0"
-module load="auvarmrc.so.0"
-module load="ausmr.so.0"
-module load=if alias="mrc"
-mrc hup
-module load=if alias="laser"
-module load=augps.so.0
-module load=mapbase.so.0
-module load=aurule.so.0
-module load=aurhdif.so.0
-rhd hup
-module load=aupoly.so.0
-# other /dev/video cameras - v4lgst in 16.04, aucamera in 18.04
-module load=aucamera.so.0
-module load=auv4lgst.so.0
-# camera parameters
-camset device=10 fps=7 width=320 gain=auto shutter=auto focallength=715
-camset device=10 posX=0.04 posy=0.0 posZ=0.3 rotPhi=0.1
-camset device=18 posX=0.04 posy=0.0 posZ=0.3 rotPhi=0.1 focallength=532
-camset device=19 posX=0.04 posy=0.0 posZ=0.3 rotPhi=0.1 focallength=532
-# ballfinder
-module load="auball.so.0"
-var ball.poolImg=18
-var ball.redHue=120
-var ball.blueHue=5
-var ball.hough="300 20 50"
-var ball.ballSize=0.15
-# some test images
-poolset img=70 loadpng="/vhome/jca/replay/31388/balls_backlight.png"
-poolset img=71 loadpng="/vhome/jca/replay/31388/balls_crosslight.png"
-poolset img=72 loadpng="/vhome/jca/replay/31388/balls_night.png"
-# end of script