├── test
    ├── data
    │   ├── cat.jpg
    │   ├── nn.jpg
    │   ├── nns.jpg
    │   ├── detect.jpg
    │   └── follow.jpg
    ├── gpst.py
    ├── rest_api
    │   ├── README
    │   ├── api_test.py
    │   └── tanq_rest.py
    └── local_test.py
├── DistCtrl.py
├── tf_labels.py
├── DnnDetectCtrl.py
├── PiConf.py
├── gpss.py
├── track_conf.py
├── roi.py
├── HaarDetectCtrl.py
├── MTNavCtrl.py
├── tf_walk.py
├── detect.py
├── PCA9685_license.txt
├── dnn_detect.py
├── MTWalkCtrl.py
├── MTFollowLineCtrl.py
├── cam.py
├── ClassifyCtrl.py
├── ultradist.py
├── geom_util.py
├── GpsUtil.py
├── stand.py
├── MotorCtrl.py
├── PhotoCtrl.py
├── README.md
├── GpsCtrl.py
├── walk_conf.py
├── track_cv.py
├── motor.py
├── string_int_label_map_pb2.py
├── FollowLineCtrl.py
├── AppCtrl.py
├── label_map_util.py
├── GpsNavCtrl.py
├── classify.py
├── app.py
├── WalkCtrl.py
└── PCA9685.py


/test/data/cat.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tprlab/pitanq/HEAD/test/data/cat.jpg


--------------------------------------------------------------------------------
/test/data/nn.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tprlab/pitanq/HEAD/test/data/nn.jpg


--------------------------------------------------------------------------------
/test/data/nns.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tprlab/pitanq/HEAD/test/data/nns.jpg


--------------------------------------------------------------------------------
/test/data/detect.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tprlab/pitanq/HEAD/test/data/detect.jpg


--------------------------------------------------------------------------------
/test/data/follow.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/tprlab/pitanq/HEAD/test/data/follow.jpg


--------------------------------------------------------------------------------
/test/gpst.py:
--------------------------------------------------------------------------------
1 | import gps
2 | 
3 | gs = gps.gps("localhost", "2947")
4 | gs.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)
5 | 
6 | for i in range(0,10):
7 |     report = gs.next()
8 |     print (report)
9 | 


--------------------------------------------------------------------------------
/DistCtrl.py:
--------------------------------------------------------------------------------
 1 | import ultradist
 2 | 
 3 | class DistCtrl:
 4 | 
 5 |     def __init__(self):
 6 |         ultradist.init()
 7 | 
 8 |     def distance(self):
 9 |         return ultradist.distance()
10 | 
11 | 
12 | def createDistCtrl():
13 |     return DistCtrl()
14 | 
15 | if __name__ == '__main__':
16 |     dc = createDistCtrl()
17 |     print (dc.distance())


--------------------------------------------------------------------------------
/test/rest_api/README:
--------------------------------------------------------------------------------
 1 | GET /ping
 2 | GET /version
 3 | GET /name
 4 | GET /dist
 5 | 
 6 | POST /fwd/on
 7 | POST /fwd/off
 8 | POST /back/on
 9 | POST /back/off
10 | POST /left/on
11 | POST /left/off
12 | POST /right/on
13 | POST /right/off
14 | 
15 | POST /photo/make
16 | GET /photo/<phid>
17 | GET /photo/list
18 | 
19 | 
20 | POST /cam/up
21 | POST /cam/down
22 | POST /cam/right
23 | POST /cam/left
24 | 
25 | POST /detect/haar/<phid>
26 | POST /detect/dnn/<phid>
27 | POST /classify/tf/<phid>
28 | 
29 | 


--------------------------------------------------------------------------------
/tf_labels.py:
--------------------------------------------------------------------------------
 1 | import label_map_util
 2 | 
 3 | 
 4 | NUM_CLASSES = 90
 5 | category_index = None
 6 | 
 7 | def initLabels(path):
 8 |     global category_index
 9 |     label_map = label_map_util.load_labelmap(path)
10 |     categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
11 |     category_index = label_map_util.create_category_index(categories)   
12 | 
13 | 
14 | def getLabel(id):
15 |     if category_index is None:
16 |         return "Not initialized"
17 |     e = category_index[id]
18 |     return e["name"] if e is not None else ""
19 | 


--------------------------------------------------------------------------------
/DnnDetectCtrl.py:
--------------------------------------------------------------------------------
 1 | import PhotoCtrl
 2 | import dnn_detect
 3 | import logging
 4 | import PiConf
 5 | 
 6 | class DnnDetectCtrl:
 7 | 
 8 |     def detect_file(self, path):
 9 |         pic, rects = dnn_detect.detectPic(path)
10 |         logging.debug("Detected on" + path + ": " + str(rects))
11 |         return rects
12 | 
13 | 
14 |     def do_detect(self, img):
15 |         path = PiConf.PHOTO_PATH + "/" + img + ".jpg"
16 |         return self.detect_file(path)
17 |             
18 | 
19 | 
20 | 
21 | def createDetectCtrl():
22 |     return DnnDetectCtrl()
23 | 
24 | 
25 | if __name__ == '__main__':
26 |     D = createDetectCtrl()
27 |     p = "test/data/detect.jpg"
28 |     rc = D.detect_file(p)
29 |     print (rc)


--------------------------------------------------------------------------------
/PiConf.py:
--------------------------------------------------------------------------------
 1 | MOTOR_RF = 40
 2 | MOTOR_RB = 38
 3 | 
 4 | MOTOR_LF = 36
 5 | MOTOR_LB = 32
 6 | 
 7 | DIST_TRIGGER = 37
 8 | DIST_ECHO = 35
 9 | 
10 | VERSION = 3.1
11 | 
12 | 
13 | DNN_PATH = "/home/pi/ssd_mobilenet_v1_coco_11_06_2017/frozen_inference_graph.pb"
14 | 
15 | DNN_TXT_PATH = "/home/pi/opencv-extra/ssd_mobilenet_v1_coco.pbtxt"
16 | 
17 | DNN_LABELS_PATH = "/home/pi/opencv-extra/mscoco_label_map.pbtxt"
18 | 
19 | CAT_CASCADE = "/home/pi/opencv-extra/haarcascades/haarcascade_frontalcatface.xml"
20 | 
21 | PHOTO_PATH="/home/pi/pitanq/photos"
22 | 
23 | LOG_PATH = "/home/pi/pitanq/logs"
24 | LOG_FILE = "robot.log"
25 | 
26 | PITANQ_HOME = "/home/pi/pitanq"
27 | 
28 | 
29 | IMAGENET_MODEL_DIR = "/home/pi/imagenet"
30 | 
31 | TMP_DIR = "/home/pi/pitanq/tmp"
32 | PYTHON_PROCESS = "/usr/bin/python3"


--------------------------------------------------------------------------------
/gpss.py:
--------------------------------------------------------------------------------
 1 | import gps
 2 | import logging
 3 | 
 4 | class gpss:
 5 |     gpss = gps.gps(host="localhost", port="2947")
 6 |     gpss.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)
 7 | 
 8 |     def getRaw(self):
 9 |         try:
10 |             return self.gpss.next()
11 |         except:
12 |             logging.exception("Cannot get gps data")
13 |         return None
14 |             
15 | 
16 |     def getCoords(self):
17 |         r = self.getRaw()
18 |         if r is None:
19 |             return None, None
20 |         print (r)
21 | 
22 |         if r['class'] == 'TPV':
23 |             if hasattr(r, 'lon'):
24 |                 return r.lat, r.lon
25 |         return None, None
26 | 
27 |     def close(self):
28 |         self.gpss.close()
29 | 
30 | 
31 | 
32 | if __name__ == '__main__': 
33 |     
34 |     g = gpss()
35 | 
36 |     for i in range(0,10):
37 |         lat, lon = g.getCoords()
38 |         print(i, lat, lon)
39 | 
40 | 


--------------------------------------------------------------------------------
/track_conf.py:
--------------------------------------------------------------------------------
 1 | ## Picture settings
 2 | 
 3 | # initial grayscale threshold
 4 | threshold = 120
 5 | 
 6 | # max grayscale threshold
 7 | threshold_max = 180
 8 | 
 9 | #min grayscale threshold
10 | threshold_min = 40
11 | 
12 | # iterations to find balanced threshold
13 | th_iterations = 10
14 | 
15 | # min % of white in roi
16 | white_min=3
17 | 
18 | # max % of white in roi
19 | white_max=12
20 | 
21 | ## Driving settings
22 | 
23 | # line angle to make a turn
24 | turn_angle = 45
25 | 
26 | # line shift to make an adjustment
27 | shift_max = 20
28 | 
29 | # turning time of shift adjustment
30 | shift_step = 0.125
31 | 
32 | # turning time of turn
33 | turn_step = 0.25
34 | 
35 | # time of straight run
36 | straight_run = 0.5
37 | 
38 | # attempts to find the line if lost
39 | find_turn_attempts = 5
40 | 
41 | # turn step to find the line if lost
42 | find_turn_step = 0.2
43 | 
44 | # max # of iterations of the whole tracking
45 | max_steps = 40
46 | 
47 | 
48 | 


--------------------------------------------------------------------------------
/roi.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import cv2 as cv
 3 | 
 4 | 
 5 | class ROI:
 6 |     area = 0
 7 |     vertices = None
 8 | 
 9 | 
10 |     def init_roi(self, width, height):
11 |         vertices = [(0, height), (width / 4, 3 * height / 4),(3 * width / 4, 3 * height / 4), (width, height),]
12 |         self.vertices = np.array([vertices], np.int32)
13 |         
14 |         blank = np.zeros((height, width, 3), np.uint8)
15 |         blank[:] = (255, 255, 255)
16 |         blank_gray = cv.cvtColor(blank, cv.COLOR_BGR2GRAY)
17 |         blank_cropped = self.crop_roi(blank_gray)
18 |         self.area = cv.countNonZero(blank_cropped)
19 | 
20 | 
21 | 
22 |     def crop_roi(self, img):
23 |         mask = np.zeros_like(img)
24 |         match_mask_color = 255
25 |         
26 |         cv.fillPoly(mask, self.vertices, match_mask_color)
27 |         masked_image = cv.bitwise_and(img, mask)
28 |         return masked_image
29 | 
30 |     def get_area(self):
31 |         return self.area
32 | 
33 |     def get_vertices(self):
34 |         return self.vertices
35 | 
36 | 


--------------------------------------------------------------------------------
/HaarDetectCtrl.py:
--------------------------------------------------------------------------------
 1 | import PhotoCtrl
 2 | import detect
 3 | import logging
 4 | import PiConf
 5 | 
 6 | class HaarDetectCtrl:
 7 | 
 8 |     cascade = PiConf.CAT_CASCADE
 9 | 
10 |     def structure(self,rects):
11 |         if rects is None:
12 |             return []
13 |         ret = []
14 |         for r in rects:
15 |             if len(r) < 4:
16 |                 continue
17 |             R = {"x" : r[0], "y" : r[1], "w" : r[2], "h" : r[3]}
18 |             ret.append(R)
19 |         logging.debug(ret)
20 |         return ret
21 | 
22 |     def detect_file(self, path):
23 |         pic, rects = detect.handleFile(path, self.cascade)
24 |         logging.debug("Detected on" + path + ": " + str(rects))
25 |         return self.structure(rects)
26 | 
27 | 
28 | 
29 |     def do_detect(self, img):
30 |         path = PiConf.PHOTO_PATH + "/" + img + ".jpg"
31 |         return self.detect_file(path)
32 |             
33 | 
34 | 
35 | 
36 | def createDetectCtrl():
37 |     return HaarDetectCtrl()
38 | 
39 | 
40 | if __name__ == '__main__':
41 |     D = createDetectCtrl()
42 |     p = "test/data/cat.jpg"
43 |     rc = D.detect_file(p)
44 |     print (rc)


--------------------------------------------------------------------------------
/MTNavCtrl.py:
--------------------------------------------------------------------------------
 1 | import threading
 2 | import time
 3 | import logging
 4 | 
 5 | import MotorCtrl
 6 | import PiConf
 7 | from GpsNavCtrl import GpsNavCtrl
 8 | 
 9 | 
10 | class MTNavCtrl(GpsNavCtrl):
11 | 
12 |     T = None
13 |     stop_flag = False
14 | 
15 | 
16 |     def start_nav(self, target):
17 |         if self.T is not None:
18 |             logging.debug(("Nav already started", self.target))
19 |             return False
20 |         self.set_target(target)
21 | 
22 |         self.stop_flag = False
23 |         self.T = threading.Thread(target=self.go_target)
24 |         self.T.start()
25 |         logging.debug(("Started nav thread", target))
26 |         return True
27 | 
28 | 
29 |     def stop_nav(self):
30 |         self.stop_flag = True
31 |         if self.T is not None:
32 |             self.T.join()
33 |             self.T = None
34 |         return True
35 | 
36 |     def go_target(self):
37 |         GpsNavCtrl.go(self, self.target)
38 |         self.T = None
39 |         logging.debug(("Nav", self.target, "finished"))
40 | 
41 | 
42 | def createNavCtrl():
43 |     return MTNavCtrl()
44 | 
45 | 
46 | if __name__ == '__main__':
47 |     nav = createNavCtrl()
48 | 
49 | 


--------------------------------------------------------------------------------
/tf_walk.py:
--------------------------------------------------------------------------------
 1 | import tensorflow as tf
 2 | import sys
 3 | import numpy as np
 4 | import os
 5 | import cv2 as cv
 6 | 
 7 | 
 8 | MODEL_JSON = "/home/pi/drive_nn/model.json"
 9 | MODEL_H5 = "/home/pi/drive_nn/model.h5"
10 | 
11 | class TFWalkClassifier:
12 |     model = None
13 |     graph = None
14 | 
15 |     TF_LEFT = 0
16 |     TF_RIGHT = 1
17 |     TF_STRAIGHT = 2
18 | 
19 |     def init(self):
20 |         f = open(MODEL_JSON, 'r')
21 |         model_data = f.read()
22 |         f.close()
23 | 
24 |         self.model = tf.keras.models.model_from_json(model_data)
25 |         self.model.load_weights(MODEL_H5)
26 |         self.graph = tf.get_default_graph()
27 | 
28 |     def classify(self, g):
29 |         g1 = np.reshape(g,[1,64,64,1])
30 |         c = None
31 |         with self.graph.as_default():
32 |             c = self.model.predict_classes(g1)
33 |             #c = self.model.predict_proba(g1)
34 |         if c is None:
35 |             return None
36 |         return c[0]
37 | 
38 | 
39 | if __name__ == '__main__':
40 |     tfc = TFWalkClassifier()
41 |     tfc.init()
42 |     path = sys.argv[1]
43 |     img = cv.imread(path, cv.IMREAD_GRAYSCALE)
44 |     c = tfc.classify(img)
45 |     print ("Classified", c)
46 | 
47 |      


--------------------------------------------------------------------------------
/detect.py:
--------------------------------------------------------------------------------
 1 | import cv2
 2 | import sys
 3 | import os
 4 | import logging
 5 | 
 6 | 
 7 | def handleFile(f, cascade):
 8 |     image = cv2.imread(f)
 9 |     if image is None:
10 |         logging.debug("File %s not found" % f)
11 |         return None, None
12 |     rects = findObj(cascade, image)
13 | 
14 |     print (f, rects)
15 |     return image, rects
16 | 
17 | 
18 | 
19 | def findObj(cascade, image):
20 |     gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
21 |  
22 |     detector = cv2.CascadeClassifier(cascade)
23 |     if detector.empty():
24 |         logging.debug("No cascade loaded: " + cascade)
25 |         return None
26 |     rects = detector.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=10, minSize=(75, 75))
27 |     return rects
28 | 
29 | def drawRects(image, rects):
30 |     for (i, (x, y, w, h)) in enumerate(rects):
31 |         cv2.rectangle(image, (x, y), (x + w, y + h), (0, 0, 255), 2)
32 | 
33 | 
34 | def showPic(title, image):
35 |     cv2.imshow(title, image)
36 |     cv2.waitKey(0)
37 | 
38 | 
39 | if __name__ == '__main__':
40 |     path = sys.argv[1]
41 |     cascade = "haarcascade_frontalcatface.xml"
42 |     img, rects = handleFile(path, cascade)
43 |     print (rects)
44 |     
45 | 


--------------------------------------------------------------------------------
/PCA9685_license.txt:
--------------------------------------------------------------------------------
 1 | https://github.com/sunfounder/SunFounder_PCA9685#license
 2 | License
 3 | This is the code for SunFounder PCA9685. 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.
 4 | 
 5 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied wa rranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 6 | 
 7 | 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 8 | 
 9 | SunFounder PCA9685 comes with ABSOLUTELY NO WARRANTY; for details run ./show w. This is free software, and you are welcome to redistribute it under certain conditions; run ./show c for details.
10 | 
11 | SunFounder, Inc., hereby disclaims all copyright interest in the program 'SunFounder PCA9685' (which makes passes at compilers).
12 | 
13 | Mike Huang, 21 August 2015
14 | 
15 | Mike Huang, Chief Executive Officer
16 | 
17 | Email: service@sunfounder.com, support@sunfounder.com


--------------------------------------------------------------------------------
/dnn_detect.py:
--------------------------------------------------------------------------------
 1 | import cv2 as cv
 2 | import tf_labels
 3 | import PiConf
 4 | import sys
 5 | 
 6 | tf_labels.initLabels(PiConf.DNN_LABELS_PATH)
 7 | 
 8 | def detectPic(img_path, thr=0.3):
 9 |     img = cv.imread(img_path)
10 |     cvNet = cv.dnn.readNetFromTensorflow(PiConf.DNN_PATH, PiConf.DNN_TXT_PATH)
11 | 
12 |     rows = img.shape[0]
13 |     cols = img.shape[1]
14 |     cvNet.setInput(cv.dnn.blobFromImage(img, 1.0/127.5, (300, 300), (127.5, 127.5, 127.5), swapRB=True, crop=False))
15 |     cvOut = cvNet.forward()
16 |     ret = []
17 | 
18 |     for detection in cvOut[0,0,:,:]:
19 |         score = float(detection[2])
20 |         if score > thr:
21 |             left = detection[3] * cols
22 |             top = detection[4] * rows
23 |             right = detection[5] * cols
24 |             bottom = detection[6] * rows
25 |             cls = int(detection[1])
26 |             a = {}
27 |             a["class"] = cls
28 |             a["name"] = tf_labels.getLabel(cls)
29 |             a["score"] = score
30 |             a["x"] = int(left)
31 |             a["y"] = int(top)
32 |             a["w"] = int(right - left)
33 |             a["h"] = int(bottom - top)
34 |             ret.append(a)
35 |     #print (ret)
36 |     return img, ret
37 | 
38 | 
39 | if __name__ == '__main__':
40 | 
41 |     d = detectPic(sys.argv[1])
42 |     print (d)


--------------------------------------------------------------------------------
/MTWalkCtrl.py:
--------------------------------------------------------------------------------
 1 | from WalkCtrl import WalkCtrl
 2 | import threading
 3 | import time
 4 | 
 5 | import MotorCtrl
 6 | import PhotoCtrl
 7 | import PiConf
 8 | 
 9 | 
10 | class MTWalkCtrl(WalkCtrl):
11 | 
12 |     T = None
13 |     stop_flag = False
14 | 
15 |     def __init__(self, motor, photo):
16 |         WalkCtrl.__init__(self, motor, photo)
17 | 
18 |     def start_follow(self):
19 |         if self.T is None or not self.T.isAlive():
20 |             self.stop_flag = False
21 |             self.init()
22 |             self.T = threading.Thread(target=self.follow)
23 |             self.T.start()
24 |         return self.track_id
25 | 
26 | 
27 |     def stop_follow(self):
28 |         self.stop_flag = True
29 |         if self.T is not None:
30 |             self.T.join()
31 |             self.T = None
32 |         return True
33 | 
34 |     def follow(self):
35 |         WalkCtrl.follow(self)
36 |         self.onDone()
37 | 
38 | 
39 |     def next(self):
40 |         if self.stop_flag:
41 |             return False
42 |         return WalkCtrl.next(self)
43 | 
44 |     def onDone(self):
45 |         self.T = None
46 | 
47 | def createWalkCtrl(motor, photo):
48 |     return MTWalkCtrl(motor, photo)
49 | 
50 | 
51 | if __name__ == '__main__':
52 | 
53 |     motor_ctrl = MotorCtrl.createMotorCtrl()
54 |     photo_ctrl = PhotoCtrl.createPhotoCtrl()
55 | 
56 |     f = MTWalkCtrl(motor_ctrl, photo_ctrl)
57 |     f.start_follow()
58 |     time.sleep(10)
59 |     f.stop_follow()


--------------------------------------------------------------------------------
/MTFollowLineCtrl.py:
--------------------------------------------------------------------------------
 1 | from FollowLineCtrl import FollowLineCtrl
 2 | import threading
 3 | import time
 4 | 
 5 | import MotorCtrl
 6 | import PhotoCtrl
 7 | import PiConf
 8 | 
 9 | 
10 | class MTFollowLineCtrl(FollowLineCtrl):
11 | 
12 |     T = None
13 |     stop_flag = False
14 | 
15 |     def __init__(self, motor, photo):
16 |         FollowLineCtrl.__init__(self, motor, photo)
17 | 
18 |     def start_follow(self):
19 |         if self.T is None or not self.T.isAlive():
20 |             self.stop_flag = False
21 |             self.init()
22 |             self.T = threading.Thread(target=self.follow)
23 |             self.T.start()
24 |         return self.track_id
25 | 
26 | 
27 |     def stop_follow(self):
28 |         self.stop_flag = True
29 |         if self.T is not None:
30 |             self.T.join()
31 |             self.T = None
32 |         return True
33 | 
34 |     def follow(self):
35 |         FollowLineCtrl.follow(self)
36 |         self.onDone()
37 | 
38 | 
39 |     def next(self):
40 |         if self.stop_flag:
41 |             return False
42 |         return FollowLineCtrl.next(self)
43 | 
44 |     def onDone(self):
45 |         self.T = None
46 | 
47 | def createLineCtrl(motor, photo):
48 |     return MTFollowLineCtrl(motor, photo)
49 | 
50 | 
51 | if __name__ == '__main__':
52 | 
53 |     motor_ctrl = MotorCtrl.createMotorCtrl()
54 |     photo_ctrl = PhotoCtrl.createPhotoCtrl()
55 | 
56 |     f = MTFollowLineCtrl(motor_ctrl, photo_ctrl)
57 |     f.start_follow()
58 |     time.sleep(10)
59 |     f.stop_follow()


--------------------------------------------------------------------------------
/cam.py:
--------------------------------------------------------------------------------
 1 | import time
 2 | import picamera
 3 | from datetime import datetime
 4 | import sys
 5 | import subprocess
 6 | 
 7 | def photo(path):
 8 |     subprocess.call(["raspistill", "-t", "1500", "-w", "640", "-h", "480", "-o", path])
 9 | 
10 | def photo_cam(path):
11 |     with picamera.PiCamera() as camera:
12 |         #camera.start_preview()
13 | 
14 |         #camera.resolution = (2592, 1944)
15 |         #camera.framerate = (1, 1)
16 | 
17 |         camera.sharpness = 0
18 |         camera.contrast = 0
19 |         camera.brightness = 50
20 |         camera.saturation = 0
21 |         camera.ISO = 0
22 |         camera.video_stabilization = False
23 |         camera.exposure_compensation = 0
24 |         camera.exposure_mode = 'auto'
25 |         camera.meter_mode = 'average'
26 |         camera.awb_mode = 'auto'
27 |         camera.image_effect = 'none'
28 |         camera.color_effects = None
29 |         camera.rotation = 0
30 |         camera.hflip = False
31 |         camera.vflip = False
32 |         camera.crop = (0.0, 0.0, 1.0, 1.0)
33 | 
34 |         camera.capture(path)
35 | 
36 |         #camera.stop_preview()
37 | 
38 | if __name__ == '__main__':    
39 |     fname = None
40 |     if len(sys.argv) > 1:
41 |         fname = sys.argv[1]
42 |     if fname is None:
43 |         tmt = datetime.now().strftime('%d%m%Y-%H%M%S')
44 |         fname = "img/" + tmt + ".jpg"
45 | 
46 |     time.sleep(0.5)
47 |     photo(fname)
48 |     print ("Saved photo", fname)
49 |         
50 | 
51 | 
52 | 


--------------------------------------------------------------------------------
/ClassifyCtrl.py:
--------------------------------------------------------------------------------
 1 | import classify
 2 | import PhotoCtrl
 3 | import PiConf
 4 | import sys
 5 | from subprocess import Popen, PIPE
 6 | import os
 7 | import json
 8 | from datetime import datetime
 9 | import logging
10 | 
11 | 
12 | 
13 | class ClassifyCtrl:
14 | 
15 |     def classify_photo(self, img):
16 |         path = PiConf.PHOTO_PATH + "/" + img + ".jpg"
17 |         return self.classify_ex(path)
18 | 
19 | 
20 |     def do_classify(self, path):
21 |         return classify.classify(path)
22 | 
23 |     def get_file_id(self):
24 |         return datetime.now().strftime('%d%m%Y%H%M%S')
25 | 
26 | 
27 |     def classify_ex(self, path):
28 |         tmp_path = PiConf.TMP_DIR + "/tf"
29 |         if not os.path.exists(tmp_path):
30 |             os.makedirs(tmp_path)
31 | 
32 |         outpath = tmp_path + "/" + self.get_file_id() + ".json"
33 |         process = Popen([PiConf.PYTHON_PROCESS, PiConf.PITANQ_HOME + "/classify.py", path, outpath], stdout=PIPE)
34 |         (output, err) = process.communicate()
35 |         exit_code = process.wait()
36 |         if not os.path.exists(outpath):
37 |             logging.debug(("No output file for TF classifier", outpath))
38 |             return None
39 |         ret = None
40 |         with open(outpath, "r") as f:
41 |             ret = json.load(f)
42 |         return ret
43 | 
44 | 
45 | 
46 | def createClassifyCtrl():
47 |     return ClassifyCtrl()
48 | 
49 | 
50 | if __name__ == '__main__':
51 |     D = createClassifyCtrl()
52 |     p = "test/data/detect.jpg"
53 |     rc = D.classify_ex(p)
54 |     print (rc)
55 | 


--------------------------------------------------------------------------------
/ultradist.py:
--------------------------------------------------------------------------------
 1 | import RPi.GPIO as GPIO
 2 | import time
 3 | import PiConf
 4 | 
 5 |  
 6 | 
 7 | def init():
 8 |     GPIO.setup(PiConf.DIST_TRIGGER, GPIO.OUT)
 9 |     GPIO.setup(PiConf.DIST_ECHO, GPIO.IN)
10 | 
11 |  
12 | def distance():
13 |     # set Trigger to HIGH
14 |     in0 = GPIO.input(PiConf.DIST_ECHO)
15 |     GPIO.output(PiConf.DIST_TRIGGER, GPIO.HIGH)
16 |  
17 |     # set Trigger after 0.01ms to LOW
18 |     time.sleep(0.00001)
19 |     GPIO.output(PiConf.DIST_TRIGGER, GPIO.LOW)
20 |  
21 |     StartTime = time.time()
22 |     StopTime = time.time()
23 | 
24 |     ok = False 
25 |     x = -1
26 |     for i in range(1, 1000):
27 |         inp = GPIO.input(PiConf.DIST_ECHO)
28 |         StartTime = time.time()
29 |         if inp == 1:        
30 |             x = i
31 |             ok = True
32 |             break
33 | 
34 |     if ok == False:
35 |         return -1
36 |     ok = False
37 |     x = -1
38 |  
39 |     for i in range(1, 5000):
40 |         inp = GPIO.input(PiConf.DIST_ECHO)
41 |         StopTime = time.time()
42 |         if inp == 0:
43 |             x = i
44 |             ok = True
45 |             break
46 | 
47 |     if ok == False:
48 |         return -2
49 | 
50 |     TimeElapsed = StopTime - StartTime
51 |     distance = (TimeElapsed * 34300) / 2
52 |  
53 |     return distance
54 |  
55 | if __name__ == '__main__':
56 |     GPIO.setwarnings(False)
57 |     GPIO.setmode(GPIO.BOARD)
58 |     init()
59 | 
60 |     try:
61 |         for i in range(0,3):
62 |             dist = distance()
63 |             print ("Measured Distance = %.1f cm" % dist)
64 |             time.sleep(1)
65 |  
66 |     except KeyboardInterrupt:
67 |         print("Measurement stopped by User")
68 |         GPIO.cleanup()


--------------------------------------------------------------------------------
/geom_util.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import math
 3 | 
 4 | 
 5 | def calc_line(x1, y1, x2, y2):
 6 |     a = float(y2 - y1) / (x2 - x1) if x2 != x1 else 0
 7 |     b = y1 - a * x1
 8 |     return a, b
 9 | 
10 | def calc_line_length(p1, p2):
11 |     dx = p1[0] - p2[0]
12 |     dy = p1[1] - p2[1]
13 |     return math.sqrt(dx * dx + dy * dy)
14 | 
15 | def get_horz_shift(x, w):
16 |     hw = w / 2
17 |     return 100 * (x - hw) / hw
18 | 
19 | 
20 | def calc_rect_area(rect_points):
21 |     a = calc_line_length(rect_points[0], rect_points[1])
22 |     b = calc_line_length(rect_points[1], rect_points[2])
23 |     return a * b
24 | 
25 | def get_vert_angle(p1, p2, w, h):
26 |     px1 = p1[0] - w/2
27 |     px2 = p2[0] - w/2
28 |     
29 |     py1 = h - p1[1]
30 |     py2 = h - p2[1]
31 | 
32 |     angle = 90
33 |     if px1 != px2:
34 |         a, b = calc_line(px1, py1, px2, py2)
35 |         angle = 0
36 |         if a != 0:
37 |             x0 = -b/a
38 |             y1 = 1.0
39 |             x1 = (y1 - b) / a
40 |             dx = x1 - x0
41 |             tg = y1 * y1 / dx / dx
42 |             angle = 180 * np.arctan(tg) / np.pi
43 |             if a < 0:
44 |                 angle = 180 - angle
45 |     return angle
46 | 
47 | 
48 | def order_box(box):
49 |     srt = np.argsort(box[:, 1])
50 |     btm1 = box[srt[0]]
51 |     btm2 = box[srt[1]]
52 | 
53 |     top1 = box[srt[2]]
54 |     top2 = box[srt[3]]
55 | 
56 |     bc = btm1[0] < btm2[0]
57 |     btm_l = btm1 if bc else btm2
58 |     btm_r = btm2 if bc else btm1
59 | 
60 |     tc = top1[0] < top2[0]
61 |     top_l = top1 if tc else top2
62 |     top_r = top2 if tc else top1
63 | 
64 |     return np.array([top_l, top_r, btm_r, btm_l])
65 | 
66 | def calc_box_vector(box):
67 |     v_side = calc_line_length(box[0], box[3])
68 |     h_side = calc_line_length(box[0], box[1])
69 |     idx = [0, 1, 2, 3]
70 |     if v_side < h_side:
71 |         idx = [0, 3, 1, 2]
72 |     return (int((box[idx[0]][0] + box[idx[1]][0]) / 2), int((box[idx[0]][1] + box[idx[1]][1]) / 2)), (int((box[idx[2]][0] + box[idx[3]][0]) / 2), int((box[idx[2]][1]  +box[idx[3]][1]) / 2))
73 | 


--------------------------------------------------------------------------------
/test/rest_api/api_test.py:
--------------------------------------------------------------------------------
 1 | import unittest
 2 | import requests
 3 | import time
 4 | import tanq_rest as tanq
 5 | 
 6 | 
 7 | class TanqTest(unittest.TestCase):
 8 | 
 9 |     def base_call(self, proc, field):
10 |         resp, code = proc()
11 |         self.assertTrue(code, requests.codes.ok)
12 |         self.assertTrue(field in resp, True)
13 | 
14 |     def print_resp(self, proc):
15 |         resp, code = proc()
16 |         print resp
17 | 
18 | 
19 | 
20 | 
21 |     def test_ping(self):
22 |         self.base_call(tanq.ping, "rc")
23 | 
24 |     def test_version(self):
25 |         self.base_call(tanq.version, "version")
26 | 
27 |     def test_dist(self):
28 |         self.base_call(tanq.dist, "rs")
29 | 
30 |     def test_name(self):
31 |         self.base_call(tanq.device_name, "name")
32 | 
33 |     def test_fwd(self):
34 |         self.base_call(tanq.fwd_on, "rc")
35 |         time.sleep(1)
36 |         self.base_call(tanq.fwd_off, "rc")
37 | 
38 |     def test_back(self):
39 |         self.base_call(tanq.back_on, "rc")
40 |         time.sleep(1)
41 |         self.base_call(tanq.back_off, "rc")
42 | 
43 |     def test_right(self):
44 |         self.base_call(tanq.right_on, "rc")
45 |         time.sleep(1)
46 |         self.base_call(tanq.right_off, "rc")
47 | 
48 |     def test_left(self):
49 |         self.base_call(tanq.left_on, "rc")
50 |         time.sleep(1)
51 |         self.base_call(tanq.left_off, "rc")
52 | 
53 |     def test_camup(self):
54 |         self.base_call(tanq.cam_up, "rc")
55 | 
56 |     def test_camdown(self):
57 |         self.base_call(tanq.cam_down, "rc")
58 | 
59 | 
60 |     def test_camleft(self):
61 |         self.base_call(tanq.cam_left, "rc")
62 | 
63 |     def test_camright(self):
64 |         self.base_call(tanq.cam_right, "rc")
65 | 
66 |     def test_make_photo(self):
67 |         self.base_call(tanq.photo, "rc")
68 | 
69 |     def test_photo_list(self):
70 |         resp, code = tanq.photo_list()
71 |         self.assertTrue(code, requests.codes.ok)
72 |         self.assertFalse(resp is None, True)
73 | 
74 | 
75 | 
76 | 
77 | 
78 | 
79 | 
80 | 
81 | 
82 |         
83 | 
84 | if __name__ == '__main__':
85 |     unittest.main()
86 | 


--------------------------------------------------------------------------------
/GpsUtil.py:
--------------------------------------------------------------------------------
 1 | import logging
 2 | import PiConf
 3 | import time
 4 | import math
 5 | 
 6 | nyc_lat = 40.7128
 7 | nyc_lon = -74.0060
 8 | 
 9 | mont_lat= 45.5017
10 | mont_lon = -73.5673
11 | 
12 | newark_lat = 40.7357
13 | newark_lon = -74.1724
14 | 
15 | sh_lat = 40.4333
16 | sh_lon = -73.9885
17 | 
18 | frg_lat = 40.7326
19 | frg_lon = -73.4454
20 | 
21 | nyc_pos = {"lat" :nyc_lat, "lon" :nyc_lon}
22 | mont_pos = {"lat" :mont_lat, "lon" :mont_lon}
23 | newark_pos = {"lat" :newark_lat, "lon" :newark_lon}
24 | sh_pos = {"lat" :sh_lat, "lon" :sh_lon}
25 | frg_pos = {"lat" :frg_lat, "lon" :frg_lon}
26 | 
27 | 
28 | 
29 | 
30 | 
31 | 
32 | def azimuth(pos1, pos2):
33 |     lat1 = toRadians(pos1["lat"])
34 |     lon1 = toRadians(pos1["lon"])
35 | 
36 |     lat2 = toRadians(pos2["lat"])
37 |     lon2 = toRadians(pos2["lon"])
38 | 
39 |     dlat = lat2 - lat1
40 |     dlon = lon2 - lon1
41 | 
42 |     x = math.sin(dlon) * math.cos(lat2)
43 |     y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
44 |     return math.atan2(x, y)
45 | 
46 | 
47 | def toRadians(a):
48 |     return a * 3.14 / 180;
49 | 
50 | def toRadians2(a):
51 |     if a < 0:
52 |         return -toRadians(-a)
53 |     arg = a if a <= 180 else a - 360
54 |     return math.radians(arg)
55 | 
56 | 
57 | 
58 | def gps_dist(pos1, pos2):
59 |     if pos1 is None or pos2 is None:
60 |         return -1
61 |     lat1 = pos1["lat"]
62 |     lon1 = pos1["lon"]
63 | 
64 |     lat2 = pos2["lat"]
65 |     lon2 = pos2["lon"]
66 | 
67 |     dlat1 = lat1 - lat2
68 |     dlon1 = lon1 - lon2
69 | 
70 |     dlat = toRadians(dlat1);
71 |     dlon = toRadians(dlon1);
72 | 
73 |     rlat1 = toRadians(lat1)
74 |     rlat2 = toRadians(lat2)
75 | 
76 |     a = math.sin(dlat / 2) * math.sin(dlat / 2) + math.cos(rlat1) * math.cos(rlat2) * math.sin(dlon / 2) * math.sin(dlon / 2);
77 | 
78 |     c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a));
79 | 
80 |     R = 6371.;
81 |     dist = R * c;
82 | 
83 |     return dist
84 |         
85 | 
86 | 
87 | 
88 | if __name__ == '__main__': 
89 | 
90 |     north_az = azimuth(nyc_pos, mont_pos)
91 |     south_az = azimuth(nyc_pos, sh_pos)
92 |     west_az = azimuth(nyc_pos, newark_pos)
93 |     east_az = azimuth(nyc_pos, frg_pos)
94 | 
95 |     print ("Azimuth: north", north_az, "south", south_az, "west", west_az, "east", east_az)    
96 |     d = gps_dist(nyc_pos, mont_pos)
97 |     print ("Dist", d)    
98 | 


--------------------------------------------------------------------------------
/stand.py:
--------------------------------------------------------------------------------
  1 | import PCA9685
  2 | import os, os.path
  3 | import cam
  4 | from datetime import datetime
  5 | import logging
  6 | import time
  7 | import traceback
  8 | 
  9 | 
 10 | 
 11 | 
 12 | servo_min = 150 
 13 | servo_max = 600
 14 | 
 15 | 
 16 | PAN_0 = 450
 17 | TILT_0 = 340
 18 | 
 19 | class StandCtrl:
 20 | 
 21 |     pwm = None
 22 |     pan_ch = 1
 23 |     tilt_ch = 0
 24 |     pan = PAN_0
 25 |     tilt = TILT_0
 26 |     pan_step = 30
 27 |     tilt_step = 30
 28 | 
 29 |     def __init__(self):
 30 |         try:
 31 |             self.pwm = PCA9685.PWM()
 32 |             self.pwm.frequency = 60
 33 |             self.pwm.write(self.pan_ch, 0, self.pan)
 34 |             self.pwm.write(self.tilt_ch, 0, self.tilt)    
 35 |         except:
 36 |             logging.debug("PWM disabled")
 37 | 
 38 | 
 39 |     def down(self):
 40 |         if self.pwm is None:
 41 |             return False
 42 |         if self.pan < servo_max:
 43 |             self.pan += self.pan_step
 44 |             self.pwm.write(self.pan_ch, 0, self.pan)
 45 |             return True
 46 |         return False
 47 |         
 48 | 
 49 |     def up(self):
 50 |         if self.pwm is None:
 51 |             return False
 52 | 
 53 |         if self.pan > servo_min:
 54 |             self.pan -= self.pan_step
 55 |             self.pwm.write(self.pan_ch, 0, self.pan)
 56 |             return True
 57 |         return False
 58 | 
 59 | 
 60 | 
 61 |     def left(self):
 62 |         if self.pwm is None:
 63 |             return False
 64 | 
 65 |         if self.tilt < servo_max:
 66 |             self.tilt += self.tilt_step
 67 |             self.pwm.write(self.tilt_ch, 0, self.tilt)
 68 |             return True
 69 |         return False
 70 | 
 71 |     def right(self):
 72 |         if self.pwm is None:
 73 |             return False
 74 | 
 75 |         if self.tilt > servo_min:
 76 |             self.tilt -= self.tilt_step
 77 |             self.pwm.write(self.tilt_ch, 0, self.tilt)
 78 |             return True
 79 |         return False
 80 | 
 81 | 
 82 |   
 83 | 
 84 | 
 85 | def createStandCtrl():
 86 |     return StandCtrl()
 87 | 
 88 | if __name__ == '__main__':    
 89 |     stand = StandCtrl()
 90 |     print ("Pan+")
 91 |     stand.up()
 92 |     time.sleep(1)
 93 |     print ("Tilt+")
 94 |     stand.right()
 95 | 
 96 |     time.sleep(1)
 97 |     print ("Pan-")
 98 |     stand.down()
 99 | 
100 |     time.sleep(1)
101 |     print ("Tilt-")
102 |     stand.left()
103 | 
104 | 


--------------------------------------------------------------------------------
/MotorCtrl.py:
--------------------------------------------------------------------------------
 1 | import motor
 2 | import logging
 3 | import time
 4 | 
 5 | 
 6 | class MotorCtrl:
 7 | 
 8 |     def __init__(self):
 9 |         motor.init()
10 | 
11 | 
12 |     def safeMotorCall(self, proc, opname):
13 |         try:
14 |             proc()
15 |             return True
16 |         except:
17 |             logging.exception(opname)
18 |         return False
19 |             
20 | 
21 |     def fwd_on(self):
22 |         return self.safeMotorCall(motor.fwd_on, "motor.fwd_on")
23 | 
24 |     def fwd_off(self):
25 |         return self.safeMotorCall(motor.fwd_off, "motor.fwd_off")
26 | 
27 |     def back_on(self):
28 |         return self.safeMotorCall(motor.back_on, "motor.back_on")
29 | 
30 |     def back_off(self):
31 |         return self.safeMotorCall(motor.back_off, "motor.back_off")
32 | 
33 |     def right_on(self):
34 |         return self.safeMotorCall(motor.right_on, "motor.right_on")
35 | 
36 |     def right_off(self):
37 |         return self.safeMotorCall(motor.right_off, "motor.right_off")
38 | 
39 |     def left_on(self):
40 |         return self.safeMotorCall(motor.left_on, "motor.left_on")
41 | 
42 |     def left_off(self):
43 |         return self.safeMotorCall(motor.left_off, "motor.left_off")
44 | 
45 |     def set_motors(self, r, l):
46 |         lc = False
47 |         rc = False
48 |         logging.debug("Set motors " + r + " " + l)
49 |         if r == "f":
50 |             rc = self.safeMotorCall(motor.motor_right_on, "motor.motor_right_on")
51 |         elif r == "s":
52 |             rc = self.safeMotorCall(motor.motor_right_off, "motor.motor_right_off")
53 |         elif r == "r":
54 |             rc = self.safeMotorCall(motor.motor_rright_on, "motor.motor_rright_on")
55 |         elif r == "h":
56 |             rc = self.safeMotorCall(motor.motor_rright_off, "motor.motor_rright_off")
57 | 
58 | 
59 |         if l == "f":
60 |             lc = self.safeMotorCall(motor.motor_left_on, "motor.motor_left_on")
61 |         elif l == "s":
62 |             lc = self.safeMotorCall(motor.motor_left_off, "motor.motor_left_off")
63 |         elif l == "r":
64 |             rc = self.safeMotorCall(motor.motor_rleft_on, "motor.motor_rleft_on")
65 |         elif l == "h":
66 |             rc = self.safeMotorCall(motor.motor_rleft_off, "motor.motor_rleft_off")
67 | 
68 |         return rc, lc
69 | 
70 | 
71 | 
72 | 
73 | 
74 | 
75 | def createMotorCtrl():
76 |     return MotorCtrl()
77 | 
78 | 
79 | if __name__ == '__main__': 
80 |     m = createMotorCtrl()
81 |     t = 2.5
82 |     m.right_on()
83 |     time.sleep(t)
84 |     m.right_off()
85 | 
86 | 


--------------------------------------------------------------------------------
/PhotoCtrl.py:
--------------------------------------------------------------------------------
 1 | import os, os.path
 2 | import cam
 3 | from datetime import datetime
 4 | import logging
 5 | import PiConf
 6 | import subprocess
 7 | 
 8 | 
 9 | 
10 | class PhotoCtrl:
11 | 
12 |     def __init__(self):
13 |         if not os.path.isdir(PiConf.PHOTO_PATH):
14 |             os.makedirs(PiConf.PHOTO_PATH)
15 | 
16 |     def get_file_id(self):
17 |         return datetime.now().strftime('%d%m%Y%H%M%S-%f')
18 | 
19 |     def make_photo(self):
20 |         phid = self.get_file_id()
21 |         path = os.path.join(PiConf.PHOTO_PATH, phid + ".jpg")
22 |         logging.debug("Making photo %s" % path)
23 |         try:
24 |             cam.photo(path)
25 |             return True, phid
26 |         except Exception as e:
27 |             logging.exception("Cannot make a photo")
28 |             return False, e.message
29 |         
30 | 
31 |     def get_path(self, phid):
32 |         f = phid + ".jpg"
33 |         path = os.path.join(PiConf.PHOTO_PATH, f)
34 |         if os.path.exists(path):
35 |             return PiConf.PHOTO_PATH, f  
36 |         return None, None
37 | 
38 |     def get_list(self):
39 |         ret = []
40 |         for file in os.listdir(PiConf.PHOTO_PATH):
41 |             if file.endswith(".jpg"):
42 |                 ret.append(os.path.splitext(file)[0])
43 |         return ret
44 | 
45 | 
46 | PHOTO_URL = "http://localhost:8080/?action=snapshot"
47 | 
48 | 
49 | class MJPhotoCtrl(PhotoCtrl):
50 |     def make_photo(self):
51 |         phid = self.get_file_id()
52 |         path = os.path.join(PiConf.PHOTO_PATH, phid + ".jpg")
53 |         logging.debug("Making mjpeg photo %s" % path)
54 |         try:
55 |             rc = subprocess.call(["wget", "-O", path, PHOTO_URL])
56 |             ok = rc == 0
57 |             if ok == False:
58 |                 return ok, None
59 |             if not os.path.exists(path):
60 |                 logging.debug(("Photo", path, "does not exist"))
61 |                 return False, None
62 |             if os.path.getsize(path) == 0:
63 |                 logging.debug(("Photo", path, "is empty"))
64 |                 try:
65 |                     os.remove(path)
66 |                 except:
67 |                     logging.error(("Cannot remove empty photo", path))
68 | 
69 |                 return False, None
70 |             return ok, phid
71 |         except Exception as e:
72 |             logging.exception("Cannot make a photo")
73 |             return False, e.message
74 |        
75 | 
76 | 
77 | def createPhotoCtrl():
78 |     return MJPhotoCtrl()
79 | 
80 | 
81 | 
82 | if __name__ == '__main__':
83 |     ph = createPhotoCtrl()
84 |     ph.make_photo()


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # PiTanq
 2 | PiTanq is a home tank built with open source hardware and powered by Raspberry Pi. 
 3 | 
 4 | Its primary purpose is to help learning AI for vehicles like self-driving, mapping, location.
 5 | 
 6 | Also it is a nice toy.
 7 | 
 8 | ![PiTanq image](https://github.com/tprlab/tprlab.github.io/blob/master/img/other/pitanq.jpg)
 9 | 
10 | ## Articles
11 | 
12 | - [Raspberry Pi Photo-tank-robot](https://medium.com/@const.toporov/raspberry-pi-photo-tank-robot-cf5ca7288adf)
13 | - [Line following robot with OpenCV and contour-based approach](https://medium.com/@const.toporov/line-following-robot-with-opencv-and-contour-based-approach-417b90f2c298)
14 | - [Robot following a walkway with OpenCV and Tensorflow](https://towardsdatascience.com/robot-following-a-walkway-with-opencv-and-tensorflow-a631eb72cb8c)
15 | - [Raspberry Pi robot with GPS](https://medium.com/@const.toporov/raspberry-pi-robot-with-gps-d6f7a9bc10a6)
16 | - [Robot following a walkway using image segmentation](https://towardsdatascience.com/robot-following-a-walkway-using-image-segmentation-272bebd93a83)
17 | - [Robot-tank with Raspberry Pi and Intel Neural Computer Stick 2](https://towardsdatascience.com/robot-tank-with-raspberry-pi-and-intel-neural-computer-stick-2-77263ca7a1c7)
18 | 
19 | ## Hardware
20 | PiTanq is based on an aluminum chassis (the motors are included).
21 | 
22 | The motors are controlled by L298N bridge.
23 | 
24 | Raspberry Pi 3 with Raspbian Jessie is a brain.
25 | 
26 | The tank equiped with 5MP PiCamera.
27 | 
28 | The camera mounted on pan-and-tilt stand managed by Raspberry via PWM controller.
29 | 
30 | The power comes from 2x1850 batteries.
31 | 
32 | Also there is an ultrasonic distance meter.
33 | 
34 | ## Software
35 | Raspbian Jessie runs on the Raspberry. 
36 | 
37 | Python service (from this repo) controls the whole tank via REST interface.
38 | 
39 | * GET /ping 
40 | * GET /version 
41 | * GET /name 
42 | * GET /dist  
43 | * POST /update
44 | * POST /fwd/on 
45 | * POST /fwd/off 
46 | * POST /back/on 
47 | * POST /back/off 
48 | * POST /left/on 
49 | * POST /left/off 
50 | * POST /right/on 
51 | * POST /right/off  
52 | * POST /photo/make 
53 | * GET /photo/:phid 
54 | * GET /photo/list  
55 | * POST /cam/up 
56 | * POST /cam/down 
57 | * POST /cam/right 
58 | * POST /cam/left  
59 | * POST /detect/haar/:phid 
60 | * POST /detect/dnn/:phid
61 | * POST /classify/tf/:phid
62 | 
63 | * POST /motor/:mode
64 | * POST /follow/start
65 | * POST /follow/stop
66 | * POST /follow/prepare
67 | * GET /follow/id
68 | * GET /follow/photo
69 | * GET /photo/track/:track_id/:ph_id 
70 | 
71 | * POST /walk/start
72 | * POST /walk/stop
73 | * POST /walk/prepare
74 | * GET /walk/photo
75 | 
76 | * GET /gps
77 | * POST /nav/start
78 | * POST /nav/stop
79 | 
80 | 
81 | This service uses AI frameworks:
82 | - Tensoflow (built by [Sam Abrahams](https://github.com/samjabrahams/tensorflow-on-raspberry-pi))
83 | - OpenCV (built by [me](https://github.com/tprlab/pi-opencv))
84 | 
85 | And third-party driver for PWM controller from [SunFounder](https://github.com/tprlab/pitanq/blob/master/PCA9685_license.txt)
86 | 
87 | ## Phone application
88 | There is an [android application](https://play.google.com/store/apps/details?id=tprlab.com.pitanq) to control the tank.
89 | Whenever I will learn IOS then an Apple app will be available. Not now.
90 | 
91 | ## Tank
92 | More info about the tank at the product [website](https://pitanq.com)
93 | 


--------------------------------------------------------------------------------
/GpsCtrl.py:
--------------------------------------------------------------------------------
  1 | from gpss import gpss
  2 | import logging
  3 | import PiConf
  4 | import threading
  5 | import subprocess
  6 | import time
  7 | 
  8 | 
  9 | class GpsCtrl:
 10 | 
 11 |     logger = None
 12 |     G = None
 13 |     version = None
 14 |     devices = None
 15 |     sats = None
 16 |     T = None
 17 |     
 18 | 
 19 |     def init_gps(self):
 20 |         self.G = gpss()
 21 |         for i in range(0,5):
 22 |             r = self.get_state()
 23 |             self.logger.debug(("Gps-init", i, r))
 24 |             if r is None:
 25 |                 continue
 26 |             if "class" not in r:
 27 |                 continue
 28 |             c = r["class"]
 29 |             if c == "VERSION":
 30 |                 self.version = r
 31 |             elif c == "SKY":
 32 |                 self.sats = r
 33 |             elif c == "DEVICES":
 34 |                 self.devices = r
 35 |             elif c == "TPV":
 36 |                 return True
 37 |         return False
 38 | 
 39 |     def init_logger(self):
 40 |         log_file = PiConf.LOG_PATH + "/gps.log"
 41 |         formatter = logging.Formatter('%(asctime)s %(levelname)s %(message)s')
 42 | 
 43 |         handler = logging.FileHandler(log_file)             
 44 |         handler.setFormatter(formatter)
 45 | 
 46 |         logger = logging.getLogger("gps")
 47 |         logger.setLevel(logging.DEBUG)
 48 |         logger.addHandler(handler)
 49 |         self.logger = logger
 50 | 
 51 |     def __init__(self):
 52 |         self.init_logger()
 53 |         rc = self.init_gps()
 54 |         logging.debug(("Gps inited", rc))
 55 |         #self.T = threading.Thread(target=self.gps_timer)
 56 |         #self.T.start()
 57 | 
 58 | 
 59 |     def get_state(self):
 60 |         return self.G.getRaw()
 61 | 
 62 |     def gps_timer(self):
 63 |         self.logger.debug("Started gps timer")
 64 |         cnt = 0
 65 |         while cnt < 10000:
 66 |             cnt += 1
 67 |             info = self.get_state()
 68 |             if info is None:
 69 |                 continue
 70 |             if not hasattr(info, "lat"):
 71 |                 continue
 72 |             self.logger.debug(("GpsTimer:", info))
 73 |             time.sleep(2)
 74 | 
 75 |     
 76 |     def get_coords(self):
 77 |         for i in range(0,5):
 78 |             s = self.get_state()
 79 |             #self.logger.debug(("GPS attempt",i, s))
 80 |             if hasattr(s, "lat"):
 81 |                 ret = {"lat" : s["lat"], "lon" : s["lon"]}
 82 |                 if hasattr(s, "track"):
 83 |                     ret["track"] = s["track"]
 84 |                 if hasattr(s, "alt"):
 85 |                     ret["alt"] = s["alt"]
 86 |                 if hasattr(s, "speed"):
 87 |                     ret["speed"] = s["speed"]
 88 | 
 89 |                 return ret
 90 |             
 91 |         return None
 92 |         
 93 | 
 94 | class MockGps:
 95 | 
 96 |     point = None
 97 | 
 98 |     def setPoint(self, pt):
 99 |         self.point = pt
100 | 
101 |     def get_coords(self):
102 |         return self.point
103 | 
104 | def createMockGpsCtrl():
105 |     return MockGps()
106 | 
107 | def createGpsCtrl():
108 |     return GpsCtrl()
109 | 
110 | if __name__ == '__main__': 
111 |     log_file = "/home/pi/gps_test.log"
112 |     logging.basicConfig(filename=log_file,level=logging.DEBUG, format='%(asctime)s.%(msecs)03d %(threadName)s %(levelname)-8s %(message)s', datefmt='%Y-%m-%d %H:%M:%S')
113 | 
114 |     g = createGpsCtrl()
115 | 
116 |     n = 0
117 |     for i in range(0,10):
118 |         print (i)
119 |         pt = g.get_coords()
120 |         if pt is not None:
121 |             n += 1
122 |             print (pt)
123 |         else:
124 |             time.sleep(1)
125 |         if n > 10:
126 |             break
127 | 


--------------------------------------------------------------------------------
/walk_conf.py:
--------------------------------------------------------------------------------
  1 | import cv2 as cv
  2 | import numpy as np
  3 | import sys
  4 | import os
  5 | import math
  6 | 
  7 | gray_EPS = 25
  8 | rgb_EPS = 25
  9 | gray_blur = 7
 10 | 
 11 | resize_blur = 15
 12 | resize_size = 64
 13 | 
 14 | white_threshold = 10
 15 | 
 16 | 
 17 | 
 18 | def blur_resize(path, outpath, blur, size):
 19 |     img = cv.imread(path)
 20 |     if img is None:
 21 |         return None, None
 22 |     h, w = img.shape[:2]
 23 | 
 24 |     blur = cv.medianBlur(img, blur)
 25 |     blur = blur[0:h, w - h:w]
 26 |     cut = cv.resize(blur, (size, size))
 27 |     if not outpath is None:
 28 |         cv.imwrite(outpath, cut)
 29 |     return img, cut
 30 | 
 31 | 
 32 | def get_clr_weight(bgr):
 33 |     x = np.argmax(bgr)
 34 |     n = np.argmin(bgr)
 35 |     if x == n:
 36 |         return 0        
 37 |     m = 3 - x - n
 38 |     d1 = int(bgr[x]) - int(bgr[m])
 39 |     d2 = int(bgr[n]) - int(bgr[m])
 40 |     return math.sqrt(d1 *d1 + d2 *d2)
 41 | 
 42 | 
 43 | def get_gray_avg(img, Q, r):
 44 |     avg = 0
 45 |     avg_n = 0
 46 |     h, w = img.shape[:2]
 47 |     for x in range(0, r):
 48 |         for y in range(h - r, h):
 49 |             bgr = img[y,x]
 50 |             D = get_clr_weight(bgr)
 51 |             #print bgr, D
 52 |             if D < Q:
 53 |                 avg += (int(bgr[0]) + int(bgr[1]) + int(bgr[2]))
 54 |                 avg_n += 3
 55 | 
 56 |     A = avg / avg_n if avg_n != 0 else 0;
 57 |     return A
 58 | 
 59 | 
 60 | 
 61 | def filter_gray(img, Q, avg, A, blur, outfile = None):
 62 |     h, w = img.shape[:2]
 63 |     gray = np.zeros((h,w,1), np.uint8)
 64 |     hblack = int(h / 3)
 65 | 
 66 |     for x in range(0, w):
 67 |         for y in range(hblack, h):
 68 |             bgr = img[y,x]
 69 |             D = get_clr_weight(bgr)
 70 |             if D < Q:
 71 |                 c = (int(bgr[0]) + int(bgr[1]) + int(bgr[2])) / 3;
 72 |                 R = abs(c - avg)
 73 |                 #print D, R
 74 |                 if R < A:
 75 |                     gray[y,x] = 255
 76 |     gray = cv.medianBlur(gray, blur)
 77 |     if outfile is not None:
 78 |         cv.imwrite(outfile, gray)
 79 |     return gray
 80 | 
 81 | def prepare_gray(path, out_path, Q, avg, A):
 82 |     img = cv.imread(path)
 83 |     gray = filter_gray(img, Q, avg, A)
 84 |     cv.imwrite(out_path + ".jpg", gray)
 85 |     return gray
 86 |     
 87 | 
 88 | 
 89 | def get_white_percent(g):
 90 |     nwh = cv.countNonZero(g)
 91 |     h, w = g.shape[:2]
 92 |     d = 100 * nwh / h / w
 93 |     return d
 94 | 
 95 | def write_word(img, word, clr):
 96 |     cv.putText(img, word,(20,30), cv.FONT_HERSHEY_SIMPLEX, 1, clr, 2, cv.LINE_AA)
 97 | 
 98 | def apply_mask_word(cpath, gpath, outpath, clr, word, clrw):
 99 |     img = apply_mask(cpath, gpath, None, clr)
100 |     if img is None:
101 |         return False
102 |     write_word(img, word, clrw)
103 |     if outpath is not None:
104 |         cv.imwrite(outpath, img)
105 |     return True
106 | 
107 | def apply_mask(cpath, gpath, outpath, clr):
108 |     cimg = cv.imread(cpath)
109 |     g = cv.imread(gpath, cv.IMREAD_GRAYSCALE)
110 |     if cimg is None or g is None:
111 |         return None
112 | 
113 |     h, w = cimg.shape[:2]
114 |     for x in range(0, w):
115 |         for y in range(0, h):
116 |             if g[y, x] != 0:
117 |                 cimg[y,x] = clr
118 |     if outpath is not None:
119 |         cv.imwrite(outpath, cimg)
120 | 
121 |     return cimg
122 | 
123 | 
124 | def to_hsv(img):
125 |     if img is None:
126 |         return None
127 |     return cv.cvtColor(img, cv.COLOR_BGR2HSV)
128 | 
129 | def get_bright(img):
130 |     return np.mean(img[:,:,2])
131 | 
132 | def get_avg_rgb(img, q):
133 |     h, w = img.shape[:2]
134 |     
135 |     b = 0 
136 |     g = 0
137 |     r = 0
138 | 
139 |     for x in range(0, q):
140 |         for y in range(h - q, h):
141 |             bgr = img[y,x]
142 |             b += bgr[0]
143 |             g += bgr[1]
144 |             r += bgr[2]
145 | 
146 |     qq = q * q
147 |     b /= qq
148 |     g /= qq
149 |     r /= qq
150 |     return r, g, b
151 | 
152 | 
153 | 
154 | if __name__ == '__main__':
155 |     img = cv.imread("all/ctrain/l/16012019142446-349856.jpg")
156 |     #avg = get_gray_avg(img, 15, 10)
157 |     #print "Avg", avg
158 |     avg = 110
159 |     gray = filter_gray(img, 12, avg, 25, 7)
160 | 
161 | 
162 |     


--------------------------------------------------------------------------------
/track_cv.py:
--------------------------------------------------------------------------------
  1 | import cv2 as cv
  2 | import numpy as np
  3 | import sys
  4 | import math
  5 | import os
  6 | import logging
  7 | import geom_util as geom
  8 | import roi
  9 | import track_conf as tconf
 10 | 
 11 |     
 12 | 
 13 | #default gray threshold
 14 | T = tconf.threshold
 15 | Roi = roi.ROI()
 16 | 
 17 | 
 18 | 
 19 | def balance_pic(image):
 20 |     global T
 21 |     ret = None
 22 |     direction = 0
 23 |     for i in range(0, tconf.th_iterations):
 24 | 
 25 |         rc, gray = cv.threshold(image, T, 255, 0)
 26 |         crop = Roi.crop_roi(gray)
 27 | 
 28 |         nwh = cv.countNonZero(crop)
 29 |         perc = int(100 * nwh / Roi.get_area())
 30 |         logging.debug(("balance attempt", i, T, perc))
 31 |         if perc > tconf.white_max:
 32 |             if T > tconf.threshold_max:
 33 |                 break
 34 |             if direction == -1:
 35 |                 ret = crop
 36 |                 break
 37 |             T += 10
 38 |             direction = 1
 39 |         elif perc < tconf.white_min:
 40 |             if T < tconf.threshold_min:
 41 |                 break
 42 |             if  direction == 1:
 43 |                 ret = crop
 44 |                 break
 45 | 
 46 |             T -= 10
 47 |             direction = -1
 48 |         else:
 49 |             ret = crop
 50 |             break  
 51 |     return ret      
 52 | 
 53 | 
 54 | def prepare_pic(image):
 55 |     global Roi
 56 |     global T
 57 |     height, width = image.shape[:2]
 58 | 
 59 |     gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)
 60 |     blurred = cv.GaussianBlur(gray, (9, 9), 0)
 61 | 
 62 |     if Roi.get_area() == 0:
 63 |         Roi.init_roi(width, height)
 64 | 
 65 |     return balance_pic(blurred), width, height
 66 | 
 67 | 
 68 | def find_main_countour(image):
 69 |     cnts, hierarchy = cv.findContours(image, cv.RETR_CCOMP, cv.CHAIN_APPROX_SIMPLE)
 70 |     C = None
 71 |     if cnts is not None and len(cnts) > 0:
 72 |          C = max(cnts, key = cv.contourArea)
 73 |     if C is None:
 74 |         return None, None
 75 |     rect = cv.minAreaRect(C)
 76 |     box = cv.boxPoints(rect)
 77 |     box = np.int0(box)
 78 |     box = geom.order_box(box)
 79 |     return C, box
 80 | 
 81 | 
 82 | 
 83 | def handle_pic(path, fout = None, show = False):
 84 |     image = cv.imread(path)
 85 |     if image is None:
 86 |         logging.warning(("File not found", path))
 87 |         return None, None
 88 |     cropped, w, h = prepare_pic(image)
 89 |     if cropped is None:
 90 |         return None, None
 91 |     cont, box = find_main_countour(cropped)
 92 |     if cont is None:
 93 |         return None, None
 94 |     
 95 |     p1, p2 = geom.calc_box_vector(box)
 96 |     if p1 is None:
 97 |         return None, None
 98 | 
 99 |     angle = geom.get_vert_angle(p1, p2, w, h)
100 |     shift = geom.get_horz_shift(p1[0], w)
101 | 
102 |     draw = fout is not None or show
103 | 
104 |     if draw:
105 |         #cv.drawContours(image, [cont], -1, (0,0,255), 3)
106 |         #cv.drawContours(image,[box],0,(255,0,0),2)
107 |         cv.line(image, p1, p2, (0, 255, 0), 3)
108 |         msg_a = "Angle {0}".format(int(angle))
109 |         msg_s = "Shift {0}".format(int(shift))
110 | 
111 |         cv.putText(image, msg_a, (10, 20), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1)
112 |         cv.putText(image, msg_s, (10, 40), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1)
113 | 
114 |     if fout is not None:
115 |         rc = cv.imwrite(fout, image)
116 | 
117 |     if show:    
118 |         cv.imshow("Image", image)
119 |         cv.waitKey(0)
120 |     return angle, shift
121 | 
122 | def check_shift_turn(angle, shift):
123 |     turn_state = 0
124 |     if angle < tconf.turn_angle or angle > 180 - tconf.turn_angle:
125 |         turn_state = np.sign(90 - angle)
126 | 
127 |     shift_state = 0
128 |     if abs(shift) > tconf.shift_max:
129 |         shift_state = np.sign(shift)
130 |     return turn_state, shift_state
131 | 
132 | def get_turn(turn_state, shift_state):
133 |     turn_dir = 0
134 |     turn_val = 0
135 |     if shift_state != 0:
136 |         turn_dir = shift_state
137 |         turn_val = tconf.shift_step if shift_state != turn_state else tconf.turn_step
138 |     elif turn_state != 0:
139 |         turn_dir = turn_state
140 |         turn_val = tconf.turn_step
141 |     return turn_dir, turn_val                
142 | 
143 | 
144 | 
145 | 
146 | if __name__ == '__main__':
147 |     logging.basicConfig(stream=sys.stdout, level=logging.DEBUG)
148 |     pic = "test/data/follow.jpg"
149 |     if len(sys.argv) > 1:
150 |         pic = sys.argv[1]
151 | 
152 |     angle, shift = handle_pic(pic, fout="out.jpg", show=False)
153 |     print ("Angle", angle, "Shift", shift)
154 |                                    


--------------------------------------------------------------------------------
/motor.py:
--------------------------------------------------------------------------------
  1 | import RPi.GPIO as GPIO
  2 | import time
  3 | import logging
  4 | import PiConf
  5 | 
  6 |  
  7 | 
  8 | LF = PiConf.MOTOR_LF
  9 | LB = PiConf.MOTOR_LB
 10 | 
 11 | RF = PiConf.MOTOR_RF
 12 | RB = PiConf.MOTOR_RB
 13 | 
 14 | motors = [RF, RB, LF, LB]
 15 | 
 16 | R_state = 0
 17 | L_state = 0
 18 | 
 19 | def init():
 20 | 
 21 |     logging.debug("Motor init")
 22 |     GPIO.setmode(GPIO.BOARD)
 23 |     GPIO.setwarnings(False)
 24 |     GPIO.cleanup() 
 25 | 
 26 | 
 27 |     for m in motors:
 28 |         GPIO.setup(m,GPIO.OUT)
 29 |         GPIO.setup(m,GPIO.LOW)
 30 | 
 31 | def stop():
 32 |     logging.debug("Motor stop")
 33 |     for m in motors:
 34 |         GPIO.setup(m,GPIO.LOW)
 35 | 
 36 | # Motor    
 37 | 
 38 | def run_motor(m, t):
 39 |     GPIO.output(m,GPIO.HIGH)
 40 |     time.sleep(t)
 41 |     GPIO.output(m,GPIO.LOW)
 42 | 
 43 | # Motor forward
 44 | 
 45 | def motor_right_on():
 46 |     logging.debug("Motor right on")
 47 |     GPIO.output(RF, GPIO.HIGH)
 48 | 
 49 | def motor_right_off():
 50 |     logging.debug("Motor right off")
 51 |     GPIO.output(RF, GPIO.LOW)
 52 | 
 53 | def motor_left_on():
 54 |     logging.debug("Motor left on")
 55 |     GPIO.output(LF, GPIO.HIGH)
 56 | 
 57 | def motor_left_off():
 58 |     logging.debug("Motor left off")
 59 |     GPIO.output(LF, GPIO.LOW)
 60 | 
 61 | # Motor reverse
 62 | 
 63 | def motor_rright_on():
 64 |     logging.debug("Motor rright on")
 65 |     GPIO.output(RB, GPIO.HIGH)
 66 | 
 67 | def motor_rright_off():
 68 |     logging.debug("Motor rright off")
 69 |     GPIO.output(RB, GPIO.LOW)
 70 | 
 71 | def motor_rleft_on():
 72 |     logging.debug("Motor rleft on")
 73 |     GPIO.output(LB, GPIO.HIGH)
 74 | 
 75 | 
 76 | def motor_rleft_off():
 77 |     logging.debug("Motor rleft off")
 78 |     GPIO.output(LB, GPIO.LOW)
 79 |  
 80 | ## Turns
 81 | 
 82 | #Left
 83 | 
 84 | def turn_left_on():
 85 |     logging.debug("Turn left on")
 86 |     motor_right_on()
 87 | 
 88 | def turn_left_off():
 89 |     logging.debug("Turn left off")
 90 |     motor_right_off()
 91 | 
 92 | def turn_xleft_on():
 93 |     logging.debug("Turn xleft on")
 94 |     motor_right_on()
 95 |     motor_rleft_on()
 96 | 
 97 | def turn_xleft_off():
 98 |     logging.debug("Turn xleft off")
 99 |     motor_right_off()
100 |     motor_rleft_off()
101 | 
102 | # Right
103 | 
104 | def turn_right_on():
105 |     logging.debug("Turn right on")
106 |     motor_left_on()
107 | 
108 | def turn_right_off():
109 |     logging.debug("Turn right off")
110 |     motor_left_off()
111 | 
112 | def turn_xright_on():
113 |     logging.debug("Turn xright on")
114 |     motor_left_on()
115 |     motor_rright_on()
116 | 
117 | def turn_xright_off():
118 |     logging.debug("Turn xright off")
119 |     motor_left_off()
120 |     motor_rright_off()
121 | 
122 | 
123 | ## Test
124 | 
125 | def forward(t):
126 |     motor_right_on()
127 |     motor_left_on()
128 |     time.sleep(t)
129 |     motor_right_off()
130 |     motor_left_off()
131 | 
132 | def back(t):
133 |     motor_rright_on()
134 |     motor_rleft_on()
135 |     time.sleep(t)
136 |     motor_rright_off()
137 |     motor_rleft_off()
138 | 
139 | def right(t):
140 |     turn_xright_on()
141 |     time.sleep(t)
142 |     turn_xright_off()
143 | 
144 | def left(t):
145 |     turn_xleft_on()
146 |     time.sleep(t)
147 |     turn_xleft_off()
148 | 
149 | # API
150 | 
151 | def fwd_on():
152 |     logging.debug("fwd on")    
153 |     motor_right_on()
154 |     motor_left_on()
155 | 
156 | def fwd_off():
157 |     logging.debug("fwd off")    
158 |     motor_right_off()
159 |     motor_left_off()
160 | 
161 | 
162 | def back_on():
163 |     logging.debug("back on")
164 |     motor_rright_on()
165 |     motor_rleft_on()
166 | 
167 | def back_off():
168 |     logging.debug("back off")
169 |     motor_rright_off()
170 |     motor_rleft_off()
171 | 
172 | 
173 | def right_on():
174 |     turn_right_on()
175 | 
176 | def right_off():
177 |     turn_right_off()
178 | 
179 | def left_on():
180 |     turn_left_on()
181 | 
182 | def left_off():
183 |     turn_left_off()
184 | 
185 | 
186 | 
187 | def back_test(t):
188 |     back_on()
189 |     time.sleep(t)
190 |     back_off()
191 | 
192 | def fwd_test(t):
193 |     fwd_on()
194 |     time.sleep(t)
195 |     fwd_off()
196 | 
197 |         
198 | 
199 | if __name__ == '__main__':    
200 |     init()
201 |     #run_motor(LB, 2) 
202 |     forward(2)
203 |     #right(2)
204 |     #back(2)
205 |     #right(3)
206 |     #left(2)
207 |     #stop()
208 |     #fwd_off()
209 | 
210 |     #back_test(2)
211 |     #fwd_test(2)
212 |     
213 |      
214 |     #GPIO.cleanup()


--------------------------------------------------------------------------------
/string_int_label_map_pb2.py:
--------------------------------------------------------------------------------
  1 | # Generated by the protocol buffer compiler.  DO NOT EDIT!
  2 | # source: string_int_label_map.proto
  3 | 
  4 | import sys
  5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
  6 | from google.protobuf import descriptor as _descriptor
  7 | from google.protobuf import message as _message
  8 | from google.protobuf import reflection as _reflection
  9 | from google.protobuf import symbol_database as _symbol_database
 10 | from google.protobuf import descriptor_pb2
 11 | # @@protoc_insertion_point(imports)
 12 | 
 13 | _sym_db = _symbol_database.Default()
 14 | 
 15 | 
 16 | 
 17 | 
 18 | DESCRIPTOR = _descriptor.FileDescriptor(
 19 |   name='string_int_label_map.proto',
 20 |   package='object_detection.protos',
 21 |   syntax='proto2',
 22 |   serialized_pb=_b('\n\x1astring_int_label_map.proto\x12\x17object_detection.protos\"G\n\x15StringIntLabelMapItem\x12\x0c\n\x04name\x18\x01 \x01(\t\x12\n\n\x02id\x18\x02 \x01(\x05\x12\x14\n\x0c\x64isplay_name\x18\x03 \x01(\t\"Q\n\x11StringIntLabelMap\x12<\n\x04item\x18\x01 \x03(\x0b\x32..object_detection.protos.StringIntLabelMapItem')
 23 | )
 24 | 
 25 | 
 26 | 
 27 | 
 28 | _STRINGINTLABELMAPITEM = _descriptor.Descriptor(
 29 |   name='StringIntLabelMapItem',
 30 |   full_name='object_detection.protos.StringIntLabelMapItem',
 31 |   filename=None,
 32 |   file=DESCRIPTOR,
 33 |   containing_type=None,
 34 |   fields=[
 35 |     _descriptor.FieldDescriptor(
 36 |       name='name', full_name='object_detection.protos.StringIntLabelMapItem.name', index=0,
 37 |       number=1, type=9, cpp_type=9, label=1,
 38 |       has_default_value=False, default_value=_b("").decode('utf-8'),
 39 |       message_type=None, enum_type=None, containing_type=None,
 40 |       is_extension=False, extension_scope=None,
 41 |       options=None, file=DESCRIPTOR),
 42 |     _descriptor.FieldDescriptor(
 43 |       name='id', full_name='object_detection.protos.StringIntLabelMapItem.id', index=1,
 44 |       number=2, type=5, cpp_type=1, label=1,
 45 |       has_default_value=False, default_value=0,
 46 |       message_type=None, enum_type=None, containing_type=None,
 47 |       is_extension=False, extension_scope=None,
 48 |       options=None, file=DESCRIPTOR),
 49 |     _descriptor.FieldDescriptor(
 50 |       name='display_name', full_name='object_detection.protos.StringIntLabelMapItem.display_name', index=2,
 51 |       number=3, type=9, cpp_type=9, label=1,
 52 |       has_default_value=False, default_value=_b("").decode('utf-8'),
 53 |       message_type=None, enum_type=None, containing_type=None,
 54 |       is_extension=False, extension_scope=None,
 55 |       options=None, file=DESCRIPTOR),
 56 |   ],
 57 |   extensions=[
 58 |   ],
 59 |   nested_types=[],
 60 |   enum_types=[
 61 |   ],
 62 |   options=None,
 63 |   is_extendable=False,
 64 |   syntax='proto2',
 65 |   extension_ranges=[],
 66 |   oneofs=[
 67 |   ],
 68 |   serialized_start=55,
 69 |   serialized_end=126,
 70 | )
 71 | 
 72 | 
 73 | _STRINGINTLABELMAP = _descriptor.Descriptor(
 74 |   name='StringIntLabelMap',
 75 |   full_name='object_detection.protos.StringIntLabelMap',
 76 |   filename=None,
 77 |   file=DESCRIPTOR,
 78 |   containing_type=None,
 79 |   fields=[
 80 |     _descriptor.FieldDescriptor(
 81 |       name='item', full_name='object_detection.protos.StringIntLabelMap.item', index=0,
 82 |       number=1, type=11, cpp_type=10, label=3,
 83 |       has_default_value=False, default_value=[],
 84 |       message_type=None, enum_type=None, containing_type=None,
 85 |       is_extension=False, extension_scope=None,
 86 |       options=None, file=DESCRIPTOR),
 87 |   ],
 88 |   extensions=[
 89 |   ],
 90 |   nested_types=[],
 91 |   enum_types=[
 92 |   ],
 93 |   options=None,
 94 |   is_extendable=False,
 95 |   syntax='proto2',
 96 |   extension_ranges=[],
 97 |   oneofs=[
 98 |   ],
 99 |   serialized_start=128,
100 |   serialized_end=209,
101 | )
102 | 
103 | _STRINGINTLABELMAP.fields_by_name['item'].message_type = _STRINGINTLABELMAPITEM
104 | DESCRIPTOR.message_types_by_name['StringIntLabelMapItem'] = _STRINGINTLABELMAPITEM
105 | DESCRIPTOR.message_types_by_name['StringIntLabelMap'] = _STRINGINTLABELMAP
106 | _sym_db.RegisterFileDescriptor(DESCRIPTOR)
107 | 
108 | StringIntLabelMapItem = _reflection.GeneratedProtocolMessageType('StringIntLabelMapItem', (_message.Message,), dict(
109 |   DESCRIPTOR = _STRINGINTLABELMAPITEM,
110 |   __module__ = 'string_int_label_map_pb2'
111 |   # @@protoc_insertion_point(class_scope:object_detection.protos.StringIntLabelMapItem)
112 |   ))
113 | _sym_db.RegisterMessage(StringIntLabelMapItem)
114 | 
115 | StringIntLabelMap = _reflection.GeneratedProtocolMessageType('StringIntLabelMap', (_message.Message,), dict(
116 |   DESCRIPTOR = _STRINGINTLABELMAP,
117 |   __module__ = 'string_int_label_map_pb2'
118 |   # @@protoc_insertion_point(class_scope:object_detection.protos.StringIntLabelMap)
119 |   ))
120 | _sym_db.RegisterMessage(StringIntLabelMap)
121 | 
122 | 
123 | # @@protoc_insertion_point(module_scope)
124 | 


--------------------------------------------------------------------------------
/test/rest_api/tanq_rest.py:
--------------------------------------------------------------------------------
  1 | import requests
  2 | import time
  3 | import json
  4 | import sys
  5 | import os
  6 | 
  7 | 
  8 | URL = "http://192.168.1.164:5000"
  9 |     
 10 | def tanq_post(path, params = None):
 11 |     headers = {'Content-type': 'application/json'}
 12 |     rsp = requests.post(URL + path, data=json.dumps(params), headers=headers)
 13 |     #print path, rsp.status_code, rsp.content
 14 |     ret = rsp.json() if rsp.status_code == requests.codes.ok else rsp.content
 15 |     return ret, rsp.status_code
 16 | 
 17 | def tanq_get(path):
 18 |     rsp = requests.get(URL + path)
 19 |     print path, rsp.status_code, rsp.content
 20 |     ret = rsp.json() if rsp.status_code == requests.codes.ok else rsp.content
 21 |     return ret, rsp.status_code
 22 | 
 23 | 
 24 | def fwd_on():
 25 |     return tanq_post("/fwd/on")
 26 | 
 27 | def fwd_off():
 28 |     return tanq_post("/fwd/off")
 29 | 
 30 | def back_on():
 31 |     return tanq_post("/back/on")
 32 | 
 33 | def back_off():
 34 |     return tanq_post("/back/off")
 35 | 
 36 | def right_on():
 37 |     return tanq_post("/right/on")
 38 | 
 39 | def right_off():
 40 |     return tanq_post("/right/off")
 41 | 
 42 | def left_on():
 43 |     return tanq_post("/left/on")
 44 | 
 45 | def left_off():
 46 |     return tanq_post("/left/off")
 47 | 
 48 | def set_motors(mode):
 49 |     return tanq_post("/motor/" + mode)
 50 | 
 51 | 
 52 | 
 53 | def photo():
 54 |     return tanq_post("/photo/make")
 55 | 
 56 | def photo_list():
 57 |     js, code = tanq_get("/photo/list")
 58 |     ret = None if code != requests.codes.ok else js["list"]
 59 |     return ret, code
 60 | 
 61 | def device_name():
 62 |     return tanq_get("/name")
 63 | 
 64 | def cam_up():
 65 |     return tanq_post("/cam/up")
 66 | 
 67 | def cam_down():
 68 |     return tanq_post("/cam/down")
 69 | 
 70 | def cam_left():
 71 |     return tanq_post("/cam/left")
 72 | 
 73 | 
 74 | def cam_right():
 75 |     return tanq_post("/cam/right")
 76 | 
 77 | def detect_haar(id):
 78 |     return tanq_post("/detect/haar/" + id)
 79 | 
 80 | def detect_dnn(id):
 81 |     return tanq_post("/detect/dnn/" + id)
 82 | 
 83 | 
 84 | def version():
 85 |     return tanq_get("/version")
 86 | 
 87 | def ping():
 88 |     return tanq_get("/ping")
 89 | 
 90 | def dist():
 91 |     return tanq_get("/dist")
 92 | 
 93 | def classify_tf(id):
 94 |     return tanq_post("/classify/tf/" + id)
 95 | 
 96 | def start_follow():
 97 |     return tanq_post("/follow/start")
 98 | 
 99 | def prepare_follow():
100 |     return tanq_post("/follow/prepare")
101 | 
102 | def stop_follow():
103 |     return tanq_post("/follow/stop")
104 | 
105 | def follow_id():
106 |     resp, rc = tanq_get("/follow/id")
107 |     return resp
108 | 
109 | def follow_photo():
110 |     resp, rc = tanq_get("/follow/photo")
111 |     return resp
112 | 
113 | def prepare_path():
114 |     return tanq_post("/path/prepare")
115 | 
116 | 
117 | def get_gps():
118 |     return tanq_get("/gps")
119 | 
120 | def start_nav():
121 |     return tanq_post("/nav/start", params={"lat":40.1, "lon" : -74.1})
122 | 
123 | 
124 | def get_photo(pid, outpath = "./"):
125 |     path = "/photo/" + pid
126 |     rsp = requests.get(URL + path, stream=True)
127 |     if rsp.status_code != requests.codes.ok:
128 |         print("No photo %s found" % pid)
129 |         return
130 | 
131 |     fname = outpath + "/" + pid + ".jpg"
132 |     slash = fname.rfind("/")
133 |     folder = fname[:slash]
134 |     print "Folder", folder
135 |     if not os.path.exists(folder):
136 |         os.makedirs(folder)
137 | 
138 |     with open(fname, "wb") as f:
139 |         for chunk in rsp.iter_content(1024):
140 |             f.write(chunk)
141 | 
142 |     print ("Saved", fname)
143 |     return fname
144 | 
145 | def test_follow_prepare():
146 |     r = prepare_follow()
147 |     print r
148 |     r = follow_photo()
149 |     print r
150 |     ph = r["photo"]
151 |     get_photo(ph)
152 |    
153 | 
154 | def test_path_prepare():
155 |     rs, rc = prepare_path()
156 |     if rc == requests.codes.ok:
157 |         get_photo(rs["path"])
158 |     else:
159 |         print rc, rs
160 |  
161 | 
162 | def test_follow():
163 |     start_follow()
164 |     fid = follow_id()
165 |     print "Started follow", fid
166 | 
167 |     for i in xrange(0, 10):
168 |         time.sleep(0.5)
169 |         pr = follow_photo()
170 |         print "Follow photo", pr
171 |         if pr["photo"] is not None:
172 |             get_photo(pr["photo"])
173 |     stop_follow()
174 | 
175 | 
176 | if __name__ == '__main__':
177 |     #print classify_tf(sys.argv[1])
178 |     #print device_name()
179 |     """
180 |     set_motors("ff")
181 |     time.sleep(2)
182 |     print("stop left")
183 |     set_motors("0s")
184 |     time.sleep(0.1)
185 |     set_motors("0f")
186 |     time.sleep(2)
187 |     print("stop all")
188 |     set_motors("ss")
189 |     """
190 |     #test_follow_prepare()
191 | 
192 |     #rs, rc = get_path_color_range()
193 |     #print rs, rc
194 | 
195 |     #rs, rc = set_path_color_range([26, 121,  59], [27, 225, 122], "hsv")
196 | 
197 |     #rs, rc = set_path_color_range([24, 120,  64], [25, 207,  96], "hsv")
198 |     #rs, rc = set_path_color_range([23, 101, 53], [28, 221, 116], "hsv")
199 | 
200 |     #rs, rc = set_path_color_range([53, 42, 7], [116, 113, 70], "rgb")
201 |     #print rc
202 |     #rs, rc = get_path_color_range()
203 |     #print rs, rc
204 |     #test_path_prepare()
205 |     ret, code = get_gps()
206 |     print code, ret
207 | 
208 |     ret, code = start_nav()
209 |     print code, ret
210 | 
211 | 


--------------------------------------------------------------------------------
/FollowLineCtrl.py:
--------------------------------------------------------------------------------
  1 | import MotorCtrl
  2 | import PhotoCtrl
  3 | import PiConf
  4 | 
  5 | import logging
  6 | import track_cv as track
  7 | import time
  8 | import numpy as np
  9 | import traceback
 10 | import math
 11 | import track_conf as tconf
 12 | from datetime import datetime
 13 | import os
 14 | 
 15 | class FollowLineCtrl:
 16 | 
 17 |     motor_ctrl = None
 18 |     photo_ctrl = None
 19 | 
 20 |     max_iter = 100
 21 | 
 22 |     last_turn = 0
 23 |     last_angle = 0 
 24 | 
 25 |     photo_n = 0
 26 | 
 27 |     path = None
 28 |     iter_n = 0
 29 | 
 30 |     track_id = None
 31 |     last_photo_path = None
 32 | 
 33 |     def next(self):
 34 |         self.iter_n += 1
 35 |         return self.iter_n < self.max_iter
 36 | 
 37 | 
 38 |     def __init__(self, motor, photo):
 39 |         self.motor_ctrl = motor
 40 |         self.photo_ctrl = photo
 41 | 
 42 | 
 43 |     def init(self):
 44 |         self.photo_n = 0
 45 |         self.iter_n = 0
 46 |         self.track_id = self.get_id()
 47 |         self.path = PiConf.PHOTO_PATH + "/track/" + self.track_id
 48 |         if not os.path.isdir(self.path):
 49 |             os.makedirs(self.path)
 50 |         return self.track_id
 51 | 
 52 | 
 53 | 
 54 |     def follow(self):
 55 |         if not self.track_id:
 56 |             self.init()
 57 | 
 58 |         self.motor_ctrl.set_motors("f", "f")   
 59 |         self.last_turn = 0
 60 |         self.last_angle = 0 
 61 | 
 62 |         try:
 63 |             while(self.next()):
 64 |                 if not self.follow_step(self.iter_n):
 65 |                     break
 66 |         finally:
 67 |             self.motor_ctrl.set_motors("s", "s")
 68 | 
 69 |         self.path = None
 70 |         self.track_id = None
 71 |         self.photo_n = 0
 72 |         self.iter_n = 0
 73 | 
 74 | 
 75 |     def follow_step(self, i):
 76 |         a, shift = self.get_vector()
 77 |         if a is None:
 78 |             if self.last_turn != 0:
 79 |                 a, shift = self.find_line(self.last_turn)
 80 |                 if a is None:
 81 |                     return False
 82 |             elif self.last_angle != 0:
 83 |                 logging.debug(("Looking for line by angle", self.last_angle))
 84 |                 self.turn(np.sign(90 - self.last_angle), tconf.turn_step)
 85 |                 return True
 86 |             else:
 87 |                 return False
 88 | 
 89 |         logging.debug((i, "Angle", a, "Shift", shift))
 90 | 
 91 |         turn_state, shift_state = track.check_shift_turn(a, shift)
 92 | 
 93 |         turn_dir, turn_val = track.get_turn(turn_state, shift_state)
 94 | 
 95 |         if turn_dir != 0:
 96 |             self.turn(turn_dir, turn_val)
 97 |             self.last_turn = turn_dir
 98 |         else:
 99 |             time.sleep(tconf.straight_run)
100 |             self.last_turn = 0
101 |         self.last_angle = a
102 |         return True
103 | 
104 | 
105 |     def find_line(self, side):
106 |         logging.debug (("Finding line", side))
107 |         if side == 0:
108 |             return None, None
109 |             
110 |         for i in xrange(0, tconf.find_turn_attempts):
111 |             self.turn(side, tconf.find_turn_step)
112 |             angle, shift = self.get_vector()
113 |             if angle is not None:
114 |                 return angle, shift
115 | 
116 |         return None, None
117 | 
118 |     def prepare_follow(self):
119 |         return self.get_vector()
120 | 
121 |     def get_photo(self):
122 |         rc, phid = self.photo_ctrl.make_photo()
123 |         if rc  == False:
124 |             return False, None, None
125 |         fpath, fname = self.photo_ctrl.get_path(phid)
126 |         if fpath is None:
127 |             return False, None, None
128 |         self.photo_n += 1
129 |         return True, phid, fpath + "/" + fname
130 | 
131 |     def get_photo_path(self, fname):
132 |         if self.path is not None:
133 |             return "{0}/{1}.jpg".format(self.path, self.photo_n)
134 |         dot = fname.rfind(".")
135 |         fpath = fname[:dot] + "-1" + fname[dot:]
136 |         return fpath
137 | 
138 |     def get_track_photo_path(self, track, photo):
139 |         return "{0}/track/{1}".format(PiConf.PHOTO_PATH, track), "{}.jpg".format(photo)
140 | 
141 | 
142 |     def get_last_photo(self):
143 |         return None if self.last_photo_path is None else self.last_photo_path[len(PiConf.PHOTO_PATH) + 1:-4]
144 | 
145 |     def get_vector(self):
146 |         rc, phid, fname = self.get_photo()
147 |         if not rc:
148 |             return None, None
149 |         self.last_photo_path = self.get_photo_path(fname)
150 |         logging.debug(("Writing track photo", self.last_photo_path))
151 |         angle, shift = track.handle_pic(fname, fout=self.last_photo_path)
152 |         if angle is None:
153 |             self.last_photo_path = fname
154 |         return angle, shift
155 | 
156 | 
157 |     def turn(self, r, t):
158 |         turn_cmd = "s0" if r > 0 else "0s"
159 |         ret_cmd = "f0" if r > 0 else "0f"
160 |         turn = "Right" if r > 0 else "Left"
161 |         logging.debug(("Turn", turn, t))
162 |         self.motor_ctrl.set_motors(turn_cmd[0], turn_cmd[1])
163 |         time.sleep(t)
164 |         self.motor_ctrl.set_motors(ret_cmd[0], ret_cmd[1])
165 | 
166 | 
167 |     def get_id(self):
168 |         return datetime.now().strftime('%d%m%Y%H%M%S')
169 | 
170 |             
171 | 
172 | if __name__ == '__main__':
173 | 
174 |     motor_ctrl = MotorCtrl.createMotorCtrl()
175 |     photo_ctrl = PhotoCtrl.createPhotoCtrl()
176 | 
177 |     f = FollowLineCtrl(motor_ctrl, photo_ctrl)
178 |     f.follow()


--------------------------------------------------------------------------------
/test/local_test.py:
--------------------------------------------------------------------------------
  1 | import unittest
  2 | import time
  3 | import warnings
  4 | import os
  5 | import logging
  6 | 
  7 | import cv2 as cv
  8 | 
  9 | import MotorCtrl
 10 | import DistCtrl
 11 | import PhotoCtrl
 12 | import HaarDetectCtrl
 13 | import DnnDetectCtrl
 14 | import ClassifyCtrl
 15 | import FollowLineCtrl
 16 | import WalkCtrl
 17 | 
 18 | import track_cv
 19 | 
 20 | 
 21 | class MotorTest(unittest.TestCase):
 22 | 
 23 |     motor_ctrl = MotorCtrl.createMotorCtrl()
 24 | 
 25 |     def test_fwd(self):
 26 |         self.motor_ctrl.fwd_on()
 27 |         self.motor_ctrl.fwd_off()
 28 | 
 29 |     def test_back(self):
 30 |         self.motor_ctrl.back_on()
 31 |         self.motor_ctrl.back_off()
 32 | 
 33 |     def test_right(self):
 34 |         self.motor_ctrl.right_on()
 35 |         self.motor_ctrl.right_off()
 36 | 
 37 |     def test_left(self):
 38 |         self.motor_ctrl.left_on()
 39 |         self.motor_ctrl.left_off()
 40 | 
 41 | 
 42 | class DistanceTest(unittest.TestCase):
 43 | 
 44 |     dist_ctrl = DistCtrl.createDistCtrl()
 45 | 
 46 |     def test_dist(self):
 47 |         d = self.dist_ctrl.distance()
 48 |         if d < 0:
 49 |             warnings.warn(("Probably distance meter does not work properly", d))
 50 | 
 51 | class PhotoTest(unittest.TestCase):
 52 | 
 53 |     photo_ctrl = PhotoCtrl.createPhotoCtrl()
 54 | 
 55 |     def test_photo(self):
 56 |         rc, phid = self.photo_ctrl.make_photo()
 57 |         self.assertEqual(rc, True, "Photo failed")
 58 |         self.assertEqual(phid is not None, True, "Photo id is none")
 59 |         path, fname = self.photo_ctrl.get_path(phid)
 60 |         self.assertEqual(path is not None, True, "Photo path is none")
 61 |         fpath = path + "/" + fname
 62 |         self.assertEqual(os.path.exists(fpath), True, "Photo file does not exist: " + fpath)
 63 |         self.assertEqual(os.path.getsize(fpath) > 0, True, "Photo file is empty")
 64 |         os.remove(fpath)
 65 | 
 66 | class HaarTest(unittest.TestCase):
 67 | 
 68 |     haar = HaarDetectCtrl.createDetectCtrl()
 69 | 
 70 |     def test_cat(self):
 71 |         path_cat = "/home/pi/pitanq/test/data/cat.jpg"
 72 |         rc = self.haar.detect_file(path_cat)
 73 |         self.assertEqual(rc is not None, True, "Cat not found")
 74 | 
 75 | 
 76 | class DnnDetectTest(unittest.TestCase):
 77 | 
 78 |     dnn = DnnDetectCtrl.createDetectCtrl()
 79 | 
 80 |     def test_detect(self):
 81 |         path = "/home/pi/pitanq/test/data/detect.jpg"
 82 |         rc = self.dnn.detect_file(path)
 83 |         self.assertEqual(rc is not None, True, "Nothing detected by DNN")
 84 |         cat_found = False
 85 |         laptop_found = False
 86 |         for r in rc:
 87 |             name = r["name"]
 88 |             if name == "cat":
 89 |                 cat_found = True
 90 |             elif name == "laptop":
 91 |                 laptop_found = True
 92 |         self.assertEqual(cat_found, True, "Cat not detected by DNN")
 93 |         self.assertEqual(laptop_found, True, "Laptop not detected by DNN")
 94 | 
 95 | 
 96 | class TFClassifyTest(unittest.TestCase):
 97 | 
 98 |     cls = ClassifyCtrl.createClassifyCtrl()
 99 | 
100 |     def test_detect(self):
101 |         path = "/home/pi/pitanq/test/data/detect.jpg"
102 |         rc = self.cls.classify_ex(path)
103 |         self.assertEqual(rc is not None, True, "Nothing detected by TF")
104 |         laptop_found = False
105 |         for r in rc:
106 |             name = r["item"]
107 |             if name.find("laptop") != -1:
108 |                 laptop_found = True
109 |         self.assertEqual(laptop_found, True, "Laptop not detected by TF")
110 | 
111 | class MockPhotoCtrl:
112 |     path = None
113 |     name = None
114 | 
115 |     def init(self, path, name):
116 |         self.path = path
117 |         self.name = name
118 | 
119 |     def make_photo(self):
120 |         return True, self.name
121 |         
122 | 
123 |     def get_path(self, phid):
124 |         return self.path, phid + ".jpg"
125 | 
126 | 
127 | class MockMotorCtrl:
128 | 
129 | 
130 |     def fwd_on(self):
131 |         return True
132 | 
133 |     def fwd_off(self):
134 |         return True
135 | 
136 |     def back_on(self):
137 |         return True
138 | 
139 |     def back_off(self):
140 |         return True
141 | 
142 |     def right_on(self):
143 |         return True
144 | 
145 |     def right_off(self):
146 |         return True
147 | 
148 |     def left_on(self):
149 |         return True
150 | 
151 |     def left_off(self):
152 |         return True
153 | 
154 | 
155 | class FollowTest(unittest.TestCase):
156 | 
157 |     def test_vector(self):
158 |         path = "/home/pi/pitanq/test/data/follow.jpg"
159 |         angle, shift = track_cv.handle_pic(path)
160 |         self.assertEqual(angle is not None, True, "Angle not found")
161 |         self.assertEqual(int(angle) == 90, True, "Invalid angle: "  + str(angle))
162 | 
163 |     def test_prepare(self):
164 |         m = MockMotorCtrl()
165 |         ph = MockPhotoCtrl()
166 |         follow = FollowLineCtrl.FollowLineCtrl(m, ph)
167 |         ph.init("/home/pi/pitanq/test/data", "follow")
168 | 
169 |         angle, shift = follow.prepare_follow()
170 |         print("Prepare", angle, shift)
171 | 
172 | class WalkTest(unittest.TestCase):
173 | 
174 |     def test_classify(self):
175 |         m = MockMotorCtrl()
176 |         ph = MockPhotoCtrl()
177 |         w = WalkCtrl.WalkCtrl(m, ph)
178 |         ph.init("/home/pi/pitanq/test/data", "nns")
179 | 
180 |         w.avg = 130
181 |         s_path = "test/data/nns.jpg"    
182 |         w.last_s_path = s_path
183 | 
184 |         img = cv.imread(s_path)
185 |         g_path = "test/data/g_nn.jpg"    
186 |         a = w.handle_small(img, g_path)
187 |         self.assertEqual(a == -1, True, "Incorrect prediction:" + str(a))
188 | 
189 | 
190 |     def test_classify_ex(self):
191 |         m = MockMotorCtrl()
192 |         ph = MockPhotoCtrl()
193 |         w = WalkCtrl.WalkCtrl(m, ph)
194 |         ph.init("/home/pi/pitanq/test/data", "nn")
195 |         rc = w.prepare_action()
196 |         self.assertEqual(rc, True, "Walk prepare failed")
197 | 
198 |         
199 |         
200 | 
201 | 
202 | if __name__ == '__main__':
203 |     log_file = "/home/pi/local_test.log"
204 |     logging.basicConfig(filename=log_file,level=logging.DEBUG, format='%(asctime)s.%(msecs)03d %(threadName)s %(levelname)-8s %(message)s', datefmt='%Y-%m-%d %H:%M:%S')
205 | 
206 |     unittest.main()
207 | 


--------------------------------------------------------------------------------
/AppCtrl.py:
--------------------------------------------------------------------------------
  1 | import MotorCtrl
  2 | import PhotoCtrl
  3 | import stand
  4 | import HaarDetectCtrl
  5 | import DnnDetectCtrl
  6 | import DistCtrl
  7 | import subprocess
  8 | import PiConf
  9 | import ClassifyCtrl
 10 | import MTFollowLineCtrl
 11 | import MTWalkCtrl
 12 | import GpsCtrl
 13 | import GpsUtil
 14 | import MTNavCtrl
 15 | 
 16 | import socket
 17 | import os
 18 | import logging
 19 | 
 20 | 
 21 | 
 22 | class AppCtrl:
 23 | 
 24 |     motor_ctrl = None
 25 |     photo_ctrl = None
 26 |     stand_ctrl = None
 27 |     haar_ctrl = None
 28 |     dnn_ctrl = None
 29 |     dist_ctrl = None
 30 |     class_ctrl = None
 31 |     line_ctrl = None
 32 |     walk_ctrl = None
 33 |     gps_ctrl = None
 34 |     nav_ctrl = None
 35 |     mock_gps_ctrl = None
 36 | 
 37 |     mock_gps = False
 38 | 
 39 |     def __init__(self):
 40 |         self.motor_ctrl = MotorCtrl.createMotorCtrl()
 41 |         self.photo_ctrl = PhotoCtrl.createPhotoCtrl()
 42 |         self.stand_ctrl = stand.createStandCtrl()
 43 |         self.dist_ctrl = DistCtrl.createDistCtrl()
 44 |         self.haar_ctrl = HaarDetectCtrl.createDetectCtrl()
 45 |         self.dnn_ctrl = DnnDetectCtrl.createDetectCtrl()
 46 |         self.class_ctrl = ClassifyCtrl.createClassifyCtrl()
 47 |         self.line_ctrl = MTFollowLineCtrl.MTFollowLineCtrl(self.motor_ctrl, self.photo_ctrl)
 48 |         self.walk_ctrl = MTWalkCtrl.createWalkCtrl(self.motor_ctrl, self.photo_ctrl)
 49 |         self.gps_ctrl = GpsCtrl.createGpsCtrl()
 50 |         self.nav_ctrl = MTNavCtrl.createNavCtrl()
 51 | 
 52 |         self.nav_ctrl.init(self.gps_ctrl, self.motor_ctrl)
 53 | 
 54 |         self.mock_gps_ctrl = GpsCtrl.createMockGpsCtrl()
 55 |         self.mock_gps_ctrl.setPoint(GpsUtil.nyc_pos)
 56 | 
 57 |         if not os.path.exists(PiConf.TMP_DIR):
 58 |             os.makedirs(PiConf.TMP_DIR)
 59 | 
 60 | 
 61 |     def ping(self):
 62 |         return {"rc" : "1"}
 63 | 
 64 |     def name(self):
 65 |         return {"name" : socket.gethostname()}
 66 | 
 67 | 
 68 |     def fwd_on(self):
 69 |         return {"rc": self.motor_ctrl.fwd_on()}
 70 | 
 71 |     def fwd_off(self):
 72 |         return {"rc" : self.motor_ctrl.fwd_off()}
 73 | 
 74 |     def back_on(self):
 75 |         return {"rc" : self.motor_ctrl.back_on()}
 76 | 
 77 |     def back_off(self):
 78 |         return {"rc" : self.motor_ctrl.back_off()}
 79 | 
 80 |     def right_on(self):
 81 |         return {"rc" : self.motor_ctrl.right_on()}
 82 | 
 83 |     def right_off(self):
 84 |         return {"rc" : self.motor_ctrl.right_off()}
 85 | 
 86 |     def left_on(self):
 87 |         return {"rc" : self.motor_ctrl.left_on()}
 88 | 
 89 |     def left_off(self):
 90 |         return {"rc" : self.motor_ctrl.left_off()}
 91 | 
 92 |     def set_motors(self, r, l):
 93 |         rc, lc = self.motor_ctrl.set_motors(r, l)
 94 |         return {"r" : rc, "l" : lc}
 95 | 
 96 | 
 97 | 
 98 |     def make_photo(self):
 99 |         rc, info = self.photo_ctrl.make_photo()
100 |         ret = {"rc" : rc}
101 |         if rc:
102 |             ret["name"] = info
103 |         else:
104 |             ret["err"] = info
105 |         return ret
106 | 
107 | 
108 | 
109 |     def getPhotoPath(self, phid):
110 |         return self.photo_ctrl.get_path(phid)
111 | 
112 |     def getPhotosList(self):
113 |         return self.photo_ctrl.get_list()
114 | 
115 |     def cam_up(self):
116 |         return {"rc" : self.stand_ctrl.up()}
117 | 
118 |     def cam_down(self):
119 |         return {"rc" : self.stand_ctrl.down()}
120 | 
121 |     def cam_left(self):
122 |         return {"rc" : self.stand_ctrl.left()}
123 | 
124 |     def cam_right(self):
125 |         return {"rc" : self.stand_ctrl.right()}
126 | 
127 |     def detect_dnn(self, ph):
128 |         return self.dnn_ctrl.do_detect(ph)
129 | 
130 |     def detect_haar(self, ph):
131 |         return self.haar_ctrl.do_detect(ph)
132 |     def dist(self):
133 |         return self.dist_ctrl.distance()
134 | 
135 | 
136 |     def update(self):
137 |         p = subprocess.Popen(["git", "pull"], cwd=PiConf.PITANQ_HOME, stdout=subprocess.PIPE)
138 |         out,err = p.communicate()
139 |         return {"rc" : p.returncode, "info":out}
140 |             
141 |     def classify(self, phid):
142 |         return self.class_ctrl.classify_photo(phid)
143 | 
144 |     def start_follow(self):
145 |         return self.line_ctrl.start_follow()
146 | 
147 |     def stop_follow(self):
148 |         return self.line_ctrl.stop_follow()
149 | 
150 |     def get_follow_photo(self):
151 |         return self.line_ctrl.get_last_photo()
152 | 
153 |     def get_track_photo_path(self, track, photo):
154 |         return self.line_ctrl.get_track_photo_path(track, photo)
155 | 
156 |     def get_follow_id(self):
157 |         return self.line_ctrl.track_id
158 | 
159 |     def prepare_follow(self):
160 |         a, s = self.line_ctrl.prepare_follow()
161 |         return {"angle" : a, "shift" : s}
162 | 
163 |     def prepare_walk(self):
164 |         rc = self.walk_ctrl.prepare_action()
165 |         return {"rc" : rc}
166 | 
167 |     def get_walk_photo(self):
168 |         return self.walk_ctrl.get_action_pic()
169 | 
170 | 
171 |     def start_walk(self):
172 |         return self.walk_ctrl.start_follow()
173 | 
174 |     def stop_walk(self):
175 |         return self.walk_ctrl.stop_follow()
176 | 
177 |     def get_mock_gps(self):
178 |         return self.mock_gps_ctrl.get_coords()
179 | 
180 |     def get_gps(self):
181 |         gps = self.gps_ctrl if self.mock_gps == False else self.mock_gps_ctrl
182 |         g = gps.get_coords()
183 |         if g is None:
184 |             return {}
185 |         logging.debug(("Returned gps", g))
186 |         return g
187 | 
188 |     def start_nav(self, target):
189 |         if self.mock_gps == False:
190 |             rc = self.nav_ctrl.start_nav(target)
191 |             return {"rc" : rc}
192 | 
193 |         self.mock_gps_ctrl.setPoint(target)
194 | 
195 |         return {"rc" : True}
196 |         
197 | 
198 |     def stop_nav(self):
199 |         if self.mock_gps == False:
200 |             return {"rc" : True}
201 |             
202 |         rc = self.nav_ctrl.stop_nav()
203 |         return {"rc" : rc}
204 | 
205 |         
206 | 
207 | 
208 | 
209 | 
210 | def createCtrl():
211 |     return AppCtrl()
212 | 
213 | 
214 | 
215 | if __name__ == '__main__':
216 |     app = createCtrl()
217 |     print (app.update())
218 | 
219 | 
220 |     


--------------------------------------------------------------------------------
/label_map_util.py:
--------------------------------------------------------------------------------
  1 | # Copyright 2017 The TensorFlow Authors. All Rights Reserved.
  2 | #
  3 | # Licensed under the Apache License, Version 2.0 (the "License");
  4 | # you may not use this file except in compliance with the License.
  5 | # You may obtain a copy of the License at
  6 | #
  7 | #     http://www.apache.org/licenses/LICENSE-2.0
  8 | #
  9 | # Unless required by applicable law or agreed to in writing, software
 10 | # distributed under the License is distributed on an "AS IS" BASIS,
 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | # See the License for the specific language governing permissions and
 13 | # limitations under the License.
 14 | # ==============================================================================
 15 | 
 16 | """Label map utility functions."""
 17 | 
 18 | import logging
 19 | 
 20 | from google.protobuf import text_format
 21 | import string_int_label_map_pb2
 22 | 
 23 | 
 24 | def _validate_label_map(label_map):
 25 |   """Checks if a label map is valid.
 26 | 
 27 |   Args:
 28 |     label_map: StringIntLabelMap to validate.
 29 | 
 30 |   Raises:
 31 |     ValueError: if label map is invalid.
 32 |   """
 33 |   for item in label_map.item:
 34 |     if item.id < 1:
 35 |       raise ValueError('Label map ids should be >= 1.')
 36 | 
 37 | 
 38 | def create_category_index(categories):
 39 |   """Creates dictionary of COCO compatible categories keyed by category id.
 40 | 
 41 |   Args:
 42 |     categories: a list of dicts, each of which has the following keys:
 43 |       'id': (required) an integer id uniquely identifying this category.
 44 |       'name': (required) string representing category name
 45 |         e.g., 'cat', 'dog', 'pizza'.
 46 | 
 47 |   Returns:
 48 |     category_index: a dict containing the same entries as categories, but keyed
 49 |       by the 'id' field of each category.
 50 |   """
 51 |   category_index = {}
 52 |   for cat in categories:
 53 |     category_index[cat['id']] = cat
 54 |   return category_index
 55 | 
 56 | 
 57 | def get_max_label_map_index(label_map):
 58 |   """Get maximum index in label map.
 59 | 
 60 |   Args:
 61 |     label_map: a StringIntLabelMapProto
 62 | 
 63 |   Returns:
 64 |     an integer
 65 |   """
 66 |   return max([item.id for item in label_map.item])
 67 | 
 68 | 
 69 | def convert_label_map_to_categories(label_map,
 70 |                                     max_num_classes,
 71 |                                     use_display_name=True):
 72 |   """Loads label map proto and returns categories list compatible with eval.
 73 | 
 74 |   This function loads a label map and returns a list of dicts, each of which
 75 |   has the following keys:
 76 |     'id': (required) an integer id uniquely identifying this category.
 77 |     'name': (required) string representing category name
 78 |       e.g., 'cat', 'dog', 'pizza'.
 79 |   We only allow class into the list if its id-label_id_offset is
 80 |   between 0 (inclusive) and max_num_classes (exclusive).
 81 |   If there are several items mapping to the same id in the label map,
 82 |   we will only keep the first one in the categories list.
 83 | 
 84 |   Args:
 85 |     label_map: a StringIntLabelMapProto or None.  If None, a default categories
 86 |       list is created with max_num_classes categories.
 87 |     max_num_classes: maximum number of (consecutive) label indices to include.
 88 |     use_display_name: (boolean) choose whether to load 'display_name' field
 89 |       as category name.  If False or if the display_name field does not exist,
 90 |       uses 'name' field as category names instead.
 91 |   Returns:
 92 |     categories: a list of dictionaries representing all possible categories.
 93 |   """
 94 |   categories = []
 95 |   list_of_ids_already_added = []
 96 |   if not label_map:
 97 |     label_id_offset = 1
 98 |     for class_id in range(max_num_classes):
 99 |       categories.append({
100 |           'id': class_id + label_id_offset,
101 |           'name': 'category_{}'.format(class_id + label_id_offset)
102 |       })
103 |     return categories
104 |   for item in label_map.item:
105 |     if not 0 < item.id <= max_num_classes:
106 |       logging.info('Ignore item %d since it falls outside of requested '
107 |                    'label range.', item.id)
108 |       continue
109 |     if use_display_name and item.HasField('display_name'):
110 |       name = item.display_name
111 |     else:
112 |       name = item.name
113 |     if item.id not in list_of_ids_already_added:
114 |       list_of_ids_already_added.append(item.id)
115 |       categories.append({'id': item.id, 'name': name})
116 |   return categories
117 | 
118 | 
119 | def load_labelmap(path):
120 |   """Loads label map proto.
121 | 
122 |   Args:
123 |     path: path to StringIntLabelMap proto text file.
124 |   Returns:
125 |     a StringIntLabelMapProto
126 |   """
127 | 
128 |   with open(path, 'r') as fid:
129 |     label_map_string = fid.read()
130 |     label_map = string_int_label_map_pb2.StringIntLabelMap()
131 |     try:
132 |       text_format.Merge(label_map_string, label_map)
133 |     except text_format.ParseError:
134 |       label_map.ParseFromString(label_map_string)
135 |   _validate_label_map(label_map)
136 |   return label_map
137 | 
138 | 
139 | def get_label_map_dict(label_map_path, use_display_name=False):
140 |   """Reads a label map and returns a dictionary of label names to id.
141 | 
142 |   Args:
143 |     label_map_path: path to label_map.
144 |     use_display_name: whether to use the label map items' display names as keys.
145 | 
146 |   Returns:
147 |     A dictionary mapping label names to id.
148 |   """
149 |   label_map = load_labelmap(label_map_path)
150 |   label_map_dict = {}
151 |   for item in label_map.item:
152 |     if use_display_name:
153 |       label_map_dict[item.display_name] = item.id
154 |     else:
155 |       label_map_dict[item.name] = item.id
156 |   return label_map_dict
157 | 
158 | 
159 | def create_category_index_from_labelmap(label_map_path):
160 |   """Reads a label map and returns a category index.
161 | 
162 |   Args:
163 |     label_map_path: Path to `StringIntLabelMap` proto text file.
164 | 
165 |   Returns:
166 |     A category index, which is a dictionary that maps integer ids to dicts
167 |     containing categories, e.g.
168 |     {1: {'id': 1, 'name': 'dog'}, 2: {'id': 2, 'name': 'cat'}, ...}
169 |   """
170 |   label_map = load_labelmap(label_map_path)
171 |   max_num_classes = max(item.id for item in label_map.item)
172 |   categories = convert_label_map_to_categories(label_map, max_num_classes)
173 |   return create_category_index(categories)
174 | 
175 | 
176 | def create_class_agnostic_category_index():
177 |   """Creates a category index with a single `object` class."""
178 |   return {1: {'id': 1, 'name': 'object'}}
179 | 


--------------------------------------------------------------------------------
/GpsNavCtrl.py:
--------------------------------------------------------------------------------
  1 | import GpsCtrl
  2 | import logging
  3 | import PiConf
  4 | import time
  5 | import math
  6 | import MotorCtrl
  7 | import GpsUtil
  8 | 
  9 | 
 10 | 
 11 | class GpsNavCtrl:
 12 | 
 13 |     gps = None
 14 |     drive = None
 15 |     target = None
 16 |     pos = None
 17 |     az = None
 18 |     TURN_HALF = 1
 19 |     
 20 |     MAX_ITERATIONS = 10
 21 |     DIST_EPS = 0.003
 22 |     STEP_LEN = 5
 23 |     AZ_LEN = 3
 24 |     ANGLE_EPS = 0.001
 25 |     
 26 |     
 27 | 
 28 |     def init(self, g, d):
 29 |         self.gps = g
 30 |         self.drive = d
 31 |         
 32 | 
 33 |     def set_target(self, pt):
 34 |         self.target = pt
 35 | 
 36 | 
 37 |     def findAzimuth(self, t):
 38 |         pos0 = self.gps.get_coords()
 39 |         if pos0 is None:
 40 |             logging.debug("Gps not ready to get azimuth")
 41 |             return None
 42 |         self.drive.fwd_on()
 43 |         time.sleep(t)
 44 |         self.drive.fwd_off()
 45 |         pos1 = self.gps.get_coords()
 46 |         if pos1 is None:
 47 |             logging.debug("Gps not ready to find azimuth")
 48 |             return None
 49 | 
 50 |         az = GpsUtil.azimuth(pos0, pos1)
 51 |         logging.debug(("Azimuth of", pos0, "to", pos1, "is", az))
 52 |         return az        
 53 | 
 54 |     def turn(self, a):
 55 |         td = 1 if a > 0 else 0
 56 |         aa = abs(a)
 57 |         tt = self.TURN_HALF * aa / 6.28
 58 |         logging.debug(("Turning for", tt, td, "to direct", a))
 59 |         on = self.drive.right_on if td > 0 else self.drive.left_on
 60 |         off = self.drive.right_off if td > 0 else self.drive.left_off
 61 |         on()
 62 |         time.sleep(tt)
 63 |         off()
 64 | 
 65 |     def forward(self, t):
 66 |         self.drive.fwd_on()
 67 |         time.sleep(t)
 68 |         self.drive.fwd_off()
 69 | 
 70 | 
 71 |     def go_step(self, t, target, pos):
 72 |         d = GpsUtil.gps_dist(pos, target)
 73 |         logging.debug(("Going to target", target, "from", pos, "distance", d))
 74 | 
 75 |         taz = GpsUtil.azimuth(pos, target)
 76 |         logging.debug(("Target azimuth", taz, "local", self.az))
 77 | 
 78 |         daz = taz - self.az
 79 |         if abs(daz) > self.ANGLE_EPS:
 80 |             self.turn(daz)
 81 |             self.az = taz
 82 | 
 83 |         self.forward(t)
 84 | 
 85 |         pos1 = self.gps.get_coords()
 86 |         if pos1 is None:
 87 |             logging.debug(("Cannot finish go iteration", target))
 88 |             return None, None
 89 | 
 90 |         track = pos1["track"] if "track" in pos1 else None
 91 |         laz = GpsUtil.azimuth(pos, pos1)
 92 |         logging.debug(("Updated direction",track, laz, self.az))
 93 | 
 94 |         if track is not None and abs(track) > self.ANGLE_EPS:
 95 |             rt = GpsUtil.toRadians2(track)
 96 |             self.az = rt
 97 |         else:
 98 |             self.az = laz
 99 | 
100 | 
101 | 
102 |         d = GpsUtil.gps_dist(pos1, target)
103 |         logging.debug(("Going to target step", target, "from", pos1, "distance", d))        
104 |         return pos1, d
105 | 
106 | 
107 |     def go(self, target):
108 |         if target is None:
109 |             return False, None, None
110 |         try:
111 |             logging.debug(("Go to target", target))
112 |             if self.az is None:
113 |                 az = self.findAzimuth(self.AZ_LEN)
114 |                 logging.debug(("Found initial azimuth", az))
115 |                 if az is None:
116 |                     return None, None,None
117 |                 self.az = az
118 | 
119 |             pos = self.gps.get_coords()
120 |             if pos is None:
121 |                 logging.debug(("Gps not ready to go to target ", target))
122 |                 return None, None, None
123 |             d= GpsUtil.gps_dist(pos, target)
124 |             logging.debug(("Initial distance", d, pos))
125 |             success = False
126 |             for i in range(0, self.MAX_ITERATIONS):
127 |                 pos, d = self.go_step(self.STEP_LEN, target, pos)
128 |                 logging.debug(("Go moved to ", i, pos, d))
129 |                 if pos is None:
130 |                     logging.debug("Go stopped because no pos")
131 |                     break
132 |                 if d < self.DIST_EPS:
133 |                     logging.debug(("Target is reached", d, pos))
134 |                     break
135 | 
136 |             logging.debug(("Final distance", d, pos, success))
137 |             return success, pos, d
138 |         except:
139 |             logging.exception("Cannot go nav")
140 |         return False, None, None
141 |        
142 | 
143 | def createGpsNavCtrl():
144 |     return GpsNavCtrl()
145 | 
146 | class MockGps:
147 |     lat1 = GpsUtil.nyc_lat
148 |     lon1 = GpsUtil.nyc_lon
149 | 
150 |     lats = {}
151 |     lons = {}
152 | 
153 |     num = 0
154 |     mode = None
155 | 
156 |     def __init__(self, mode):
157 |         self.lats["n"] = GpsUtil.mont_lat
158 |         self.lons["n"] = GpsUtil.mont_lon
159 | 
160 | 
161 |         self.lats["e"] = GpsUtil.frg_lat
162 |         self.lons["e"] = GpsUtil.frg_lon
163 | 
164 |         self.lats["s"] = GpsUtil.sh_lat
165 |         self.lons["s"] = GpsUtil.sh_lon
166 | 
167 |         self.lats["w"] = GpsUtil.newark_lat
168 |         self.lons["w"] = GpsUtil.newark_lon
169 | 
170 |         self.mode = mode
171 |     
172 |     def get_coords(self):
173 |         if self.num == 0:
174 |             self.num += 1
175 |             return {"lat" : self.lat1, "lon" : self.lon1}
176 |         elif self.num == 1:
177 |             self.num += 1
178 |             return {"lat" : self.lats[self.mode], "lon" : self.lons[self.mode]}
179 |         return None
180 | 
181 | class MockDrive:
182 |     def fwd_on(self):
183 |         return True
184 | 
185 |     def fwd_off(self):
186 |         return True
187 | 
188 | 
189 | 
190 | 
191 | if __name__ == '__main__': 
192 | 
193 |     log_file = "/home/pi/gps_nav_test.log"
194 |     logging.basicConfig(filename=log_file,level=logging.DEBUG, format='%(asctime)s.%(msecs)03d %(threadName)s %(levelname)-8s %(message)s', datefmt='%Y-%m-%d %H:%M:%S')
195 | 
196 |     target = GpsUtil.nyc_pos
197 |                 
198 |     nav = createGpsNavCtrl()
199 |     gps = GpsCtrl.createGpsCtrl()
200 |     drive = MotorCtrl.createMotorCtrl()
201 |     nav.init(gps, drive)
202 | 
203 |     pt = None
204 |     for i in range(0,5):
205 |         pt = gps.get_coords()
206 |         if pt is not None:
207 |             break
208 |     if pt is not None:
209 |         logging.debug(("Got initial position", pt))
210 |         rc, pos, dist = nav.go(target)
211 |         print ("Go results", rc, pos, dist)
212 |     
213 |     """
214 | 
215 |     mgps = MockGps("n")
216 |     md = MockDrive()
217 |     nav.init(mgps, md)
218 |     """
219 | 


--------------------------------------------------------------------------------
/classify.py:
--------------------------------------------------------------------------------
  1 | # Copyright 2015 The TensorFlow Authors. All Rights Reserved.
  2 | #
  3 | # Licensed under the Apache License, Version 2.0 (the "License");
  4 | # you may not use this file except in compliance with the License.
  5 | # You may obtain a copy of the License at
  6 | #
  7 | #     http://www.apache.org/licenses/LICENSE-2.0
  8 | #
  9 | # Unless required by applicable law or agreed to in writing, software
 10 | # distributed under the License is distributed on an "AS IS" BASIS,
 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | # See the License for the specific language governing permissions and
 13 | # limitations under the License.
 14 | # ==============================================================================
 15 | 
 16 | """Simple image classification with Inception.
 17 | 
 18 | Run image classification with Inception trained on ImageNet 2012 Challenge data
 19 | set.
 20 | 
 21 | This program creates a graph from a saved GraphDef protocol buffer,
 22 | and runs inference on an input JPEG image. It outputs human readable
 23 | strings of the top 5 predictions along with their probabilities.
 24 | 
 25 | Change the --image_file argument to any jpg image to compute a
 26 | classification of that image.
 27 | 
 28 | Please see the tutorial and website for a detailed description of how
 29 | to use this script to perform image recognition.
 30 | 
 31 | https://tensorflow.org/tutorials/image_recognition/
 32 | """
 33 | 
 34 | import argparse
 35 | import os.path
 36 | import re
 37 | import sys
 38 | import traceback
 39 | import json
 40 | 
 41 | import numpy as np
 42 | import tensorflow as tf
 43 | import PiConf
 44 | 
 45 | model_dir = PiConf.IMAGENET_MODEL_DIR
 46 | num_top_predictions = 10
 47 | 
 48 | label_path = 'imagenet_2012_challenge_label_map_proto.pbtxt'
 49 | uid_path = 'imagenet_synset_to_human_label_map.txt'
 50 | nn_name = 'classify_image_graph_def.pb'
 51 | 
 52 | class NodeLookup(object):
 53 |   """Converts integer node ID's to human readable labels."""
 54 | 
 55 |   def __init__(self,
 56 |                label_lookup_path=None,
 57 |                uid_lookup_path=None):
 58 |     if not label_lookup_path:
 59 |       label_lookup_path = os.path.join(model_dir, label_path)
 60 |     if not uid_lookup_path:
 61 |       uid_lookup_path = os.path.join(model_dir, uid_path)
 62 |     self.node_lookup = self.load(label_lookup_path, uid_lookup_path)
 63 | 
 64 |   def load(self, label_lookup_path, uid_lookup_path):
 65 |     """Loads a human readable English name for each softmax node.
 66 | 
 67 |     Args:
 68 |       label_lookup_path: string UID to integer node ID.
 69 |       uid_lookup_path: string UID to human-readable string.
 70 | 
 71 |     Returns:
 72 |       dict from integer node ID to human-readable string.
 73 |     """
 74 |     if not tf.gfile.Exists(uid_lookup_path):
 75 |       tf.logging.fatal('File does not exist %s', uid_lookup_path)
 76 |     if not tf.gfile.Exists(label_lookup_path):
 77 |       tf.logging.fatal('File does not exist %s', label_lookup_path)
 78 | 
 79 |     # Loads mapping from string UID to human-readable string
 80 |     proto_as_ascii_lines = tf.gfile.GFile(uid_lookup_path).readlines()
 81 |     uid_to_human = {}
 82 |     p = re.compile(r'[n\d]*[ \S,]*')
 83 |     for line in proto_as_ascii_lines:
 84 |       parsed_items = p.findall(line)
 85 |       uid = parsed_items[0]
 86 |       human_string = parsed_items[2]
 87 |       uid_to_human[uid] = human_string
 88 | 
 89 |     # Loads mapping from string UID to integer node ID.
 90 |     node_id_to_uid = {}
 91 |     proto_as_ascii = tf.gfile.GFile(label_lookup_path).readlines()
 92 |     for line in proto_as_ascii:
 93 |       if line.startswith('  target_class:'):
 94 |         target_class = int(line.split(': ')[1])
 95 |       if line.startswith('  target_class_string:'):
 96 |         target_class_string = line.split(': ')[1]
 97 |         node_id_to_uid[target_class] = target_class_string[1:-2]
 98 | 
 99 |     # Loads the final mapping of integer node ID to human-readable string
100 |     node_id_to_name = {}
101 |     for key, val in node_id_to_uid.items():
102 |       if val not in uid_to_human:
103 |         tf.logging.fatal('Failed to locate: %s', val)
104 |       name = uid_to_human[val]
105 |       node_id_to_name[key] = name
106 | 
107 |     return node_id_to_name
108 | 
109 |   def id_to_string(self, node_id):
110 |     if node_id not in self.node_lookup:
111 |       return ''
112 |     return self.node_lookup[node_id]
113 | 
114 | 
115 | def create_graph():
116 |   """Creates a graph from saved GraphDef file and returns a saver."""
117 |   # Creates graph from saved graph_def.pb.
118 | 
119 |   with tf.gfile.FastGFile(os.path.join(model_dir, nn_name), 'rb') as f:
120 |     graph_def = tf.GraphDef()
121 |     graph_def.ParseFromString(f.read())
122 |     _ = tf.import_graph_def(graph_def, name='')
123 |     print (type(graph_def))
124 | 
125 | 
126 | def run_inference_on_image(image):
127 |   """Runs inference on an image.
128 | 
129 |   Args:
130 |     image: Image file name.
131 | 
132 |   Returns:
133 |     list of versions
134 |   """
135 |   if not tf.gfile.Exists(image):
136 |     tf.logging.fatal('File does not exist %s', image)
137 |   image_data = tf.gfile.FastGFile(image, 'rb').read()
138 | 
139 |   # Creates graph from saved GraphDef.
140 |   create_graph()
141 | 
142 |   with tf.Session() as sess:
143 |     # Some useful tensors:
144 |     # 'softmax:0': A tensor containing the normalized prediction across
145 |     #   1000 labels.
146 |     # 'pool_3:0': A tensor containing the next-to-last layer containing 2048
147 |     #   float description of the image.
148 |     # 'DecodeJpeg/contents:0': A tensor containing a string providing JPEG
149 |     #   encoding of the image.
150 |     # Runs the softmax tensor by feeding the image_data as input to the graph.
151 |     softmax_tensor = sess.graph.get_tensor_by_name('softmax:0')
152 |     predictions = sess.run(softmax_tensor,
153 |                            {'DecodeJpeg/contents:0': image_data})
154 |     predictions = np.squeeze(predictions)
155 | 
156 |     # Creates node ID --> English string lookup.
157 |     node_lookup = NodeLookup()
158 | 
159 |     top_k = predictions.argsort()[-num_top_predictions:][::-1]
160 |     ret = []
161 |     for node_id in top_k:
162 |       human_string = node_lookup.id_to_string(node_id)
163 |       score = predictions[node_id]
164 |       ret.append({"item" : human_string, "score" : float(score)})
165 |     return ret
166 |         
167 | def classify(img):
168 |     return run_inference_on_image(img)
169 | 
170 | 
171 | def main(_):
172 | 
173 |   image = sys.argv[1]
174 |   ret = run_inference_on_image(image)
175 |   print (ret)
176 |   if len(sys.argv) > 2:
177 |     with open(sys.argv[2], 'w') as outfile:
178 |         json.dump(ret, outfile)    
179 | 
180 | 
181 | 
182 | if __name__ == '__main__':
183 |   tf.app.run(main=main)
184 | 


--------------------------------------------------------------------------------
/app.py:
--------------------------------------------------------------------------------
  1 | from flask import Flask, jsonify, request, send_from_directory
  2 | import requests
  3 | import logging
  4 | import time
  5 | import json
  6 | import os
  7 | 
  8 | import PiConf
  9 | import AppCtrl
 10 | 
 11 | 
 12 | if not os.path.isdir(PiConf.LOG_PATH):
 13 |     os.makedirs(PiConf.LOG_PATH)        
 14 | 
 15 | log_file = PiConf.LOG_PATH + "/" + PiConf.LOG_FILE
 16 | logging.basicConfig(filename=log_file,level=logging.DEBUG, format='%(asctime)s.%(msecs)03d %(threadName)s %(levelname)-8s %(message)s', datefmt='%Y-%m-%d %H:%M:%S')
 17 | 
 18 | app = Flask(__name__)
 19 | app_ctrl = AppCtrl.createCtrl()
 20 | 
 21 | @app.route('/')
 22 | def index():
 23 |     return 'Hello world'
 24 | 
 25 | 
 26 | @app.route('/ping', methods=['GET'])
 27 | def ping():
 28 |     return jsonify(app_ctrl.ping()), requests.codes.ok
 29 | 
 30 | @app.route('/version', methods=['GET'])
 31 | def version():
 32 |     return jsonify({"version" : PiConf.VERSION}), requests.codes.ok
 33 | 
 34 | 
 35 | @app.route('/name', methods=['GET'])
 36 | def name():
 37 |     return jsonify(app_ctrl.name()), requests.codes.ok
 38 | 
 39 | 
 40 | 
 41 | 
 42 | @app.route('/fwd/on', methods=['POST'])
 43 | def forward_on():
 44 |     return jsonify(app_ctrl.fwd_on()), requests.codes.ok
 45 | 
 46 | @app.route('/fwd/off', methods=['POST'])
 47 | def forward_off():
 48 |     return jsonify(app_ctrl.fwd_off()), requests.codes.ok
 49 | 
 50 | @app.route('/motor/<mode>', methods=['POST'])
 51 | def set_motors(mode):
 52 |     if len(mode) < 2:
 53 |         return jsonify({"rc" : False, "lc" : False}), requests.codes.ok
 54 |         
 55 |     return jsonify(app_ctrl.set_motors(mode[0], mode[1])), requests.codes.ok
 56 | 
 57 | 
 58 | 
 59 | @app.route('/back/on', methods=['POST'])
 60 | def backward_on():
 61 |     return jsonify(app_ctrl.back_on()), requests.codes.ok
 62 | 
 63 | @app.route('/back/off', methods=['POST'])
 64 | def backward_off():
 65 |     return jsonify(app_ctrl.back_off()), requests.codes.ok
 66 | 
 67 | 
 68 | @app.route('/left/on', methods=['POST'])
 69 | def left_on():
 70 |     return jsonify(app_ctrl.left_on()), requests.codes.ok
 71 | 
 72 | @app.route('/left/off', methods=['POST'])
 73 | def left_off():
 74 |     return jsonify(app_ctrl.left_off()), requests.codes.ok
 75 | 
 76 | 
 77 | 
 78 | @app.route('/right/on', methods=['POST'])
 79 | def right_on():
 80 |     return jsonify(app_ctrl.right_on()), requests.codes.ok
 81 | 
 82 | @app.route('/right/off', methods=['POST'])
 83 | def right_off():
 84 |     return jsonify(app_ctrl.right_off()), requests.codes.ok
 85 | 
 86 | @app.route('/photo/make', methods=['POST'])
 87 | def make_photo():
 88 |     return jsonify(app_ctrl.make_photo()), requests.codes.ok
 89 | 
 90 | @app.route('/photo/<phid>', methods=['GET'])
 91 | def get_photo(phid):
 92 |     path, filename = app_ctrl.getPhotoPath(phid)
 93 |     if path is None:
 94 |         return "File not found", requests.codes.not_found
 95 |     return send_from_directory(directory=path, filename=filename)
 96 | 
 97 | @app.route('/photo/track/<tid>/<phid>', methods=['GET'])
 98 | def get_track_photo(tid, phid):
 99 |     path, filename = app_ctrl.get_track_photo_path(tid, phid)
100 |     if path is None:
101 |         return "File not found", requests.codes.not_found
102 |     return send_from_directory(directory=path, filename=filename)
103 | 
104 | 
105 | @app.route('/photo/list', methods=['GET'])
106 | def get_photos():
107 |     lst = app_ctrl.getPhotosList()
108 |     return jsonify({"list" : lst}), requests.codes.ok
109 | 
110 | 
111 | @app.route('/cam/up', methods=['POST'])
112 | def cam_up():
113 |     return jsonify(app_ctrl.cam_up()), requests.codes.ok
114 | 
115 | @app.route('/cam/down', methods=['POST'])
116 | def cam_down():
117 |     return jsonify(app_ctrl.cam_down()), requests.codes.ok
118 | 
119 | @app.route('/cam/right', methods=['POST'])
120 | def cam_right():
121 |     return jsonify(app_ctrl.cam_right()), requests.codes.ok
122 | 
123 | @app.route('/cam/left', methods=['POST'])
124 | def cam_left():
125 |     return jsonify(app_ctrl.cam_left()), requests.codes.ok
126 | 
127 | @app.route('/detect/haar/<phid>', methods=['POST'])
128 | def detect_haar(phid):
129 |     return jsonify({"rects" : app_ctrl.detect_haar(phid)}), requests.codes.ok
130 | 
131 | @app.route('/detect/dnn/<phid>', methods=['POST'])
132 | def detect_dnn(phid):
133 |     return jsonify({"rs" : app_ctrl.detect_dnn(phid)}), requests.codes.ok
134 | 
135 | @app.route('/classify/tf/<phid>', methods=['POST'])
136 | def classify_tf(phid):
137 |     return jsonify({"rs" : app_ctrl.classify(phid)}), requests.codes.ok
138 | 
139 | 
140 | @app.route('/update', methods=['POST'])
141 | def update():
142 |     return jsonify(app_ctrl.update()), requests.codes.ok
143 | 
144 | 
145 | @app.route('/follow/start', methods=['POST'])
146 | def start_follow():
147 |     return jsonify({"id" : app_ctrl.start_follow()}), requests.codes.ok
148 | 
149 | 
150 | @app.route('/follow/stop', methods=['POST'])
151 | def stop_follow():
152 |     return jsonify({"rs" : app_ctrl.stop_follow()}), requests.codes.ok
153 | 
154 | @app.route('/follow/prepare', methods=['POST'])
155 | def prepare_follow():
156 |     return jsonify(app_ctrl.prepare_follow()), requests.codes.ok
157 | 
158 | 
159 | @app.route('/follow/id', methods=['GET'])
160 | def get_follow_id():
161 |     return jsonify({"id" : app_ctrl.get_follow_id()}), requests.codes.ok
162 | 
163 | @app.route('/follow/photo', methods=['GET'])
164 | def get_follow_photo():
165 |     return jsonify({"photo" : app_ctrl.get_follow_photo()}), requests.codes.ok
166 | 
167 | 
168 | @app.route('/walk/start', methods=['POST'])
169 | def start_walk():
170 |     return jsonify({"id" : app_ctrl.start_walk()}), requests.codes.ok
171 | 
172 | 
173 | @app.route('/walk/stop', methods=['POST'])
174 | def stop_walk():
175 |     return jsonify({"rs" : app_ctrl.stop_walk()}), requests.codes.ok
176 | 
177 | 
178 | @app.route('/walk/prepare', methods=['POST'])
179 | def prepare_walk():
180 |     return jsonify(app_ctrl.prepare_walk()), requests.codes.ok
181 | 
182 | 
183 | @app.route('/walk/photo', methods=['GET'])
184 | def get_walk_photo():
185 |     return jsonify({"photo" : app_ctrl.get_walk_photo()}), requests.codes.ok
186 | 
187 | 
188 | 
189 | @app.route('/dist', methods=['GET'])
190 | def dist():
191 |     return jsonify({"rs" : app_ctrl.dist()}), requests.codes.ok
192 | 
193 | @app.route('/gps', methods=['GET'])
194 | def gps():
195 |     return jsonify(app_ctrl.get_gps()), requests.codes.ok
196 | 
197 | @app.route('/nav/start', methods=['POST'])
198 | def start_nav():
199 |     content = request.json
200 |     if not "lat" in content:
201 |         return jsonify({"missed" : "lat"}), requests.codes.bad_request
202 | 
203 |     if not "lon" in content:
204 |         return jsonify({"missed" : "lon"}), requests.codes.bad_request
205 | 
206 |     return jsonify(app_ctrl.start_nav(content)), requests.codes.ok
207 | 
208 | @app.route('/nav/stop', methods=['POST'])
209 | def stop_nav():
210 |     return jsonify(app_ctrl.stop_nav()), requests.codes.ok
211 | 
212 | 
213 | 
214 | 
215 | 
216 | if __name__ == '__main__':
217 |     app.run(debug=True, host='0.0.0.0')


--------------------------------------------------------------------------------
/WalkCtrl.py:
--------------------------------------------------------------------------------
  1 | import MotorCtrl
  2 | import PhotoCtrl
  3 | import PiConf
  4 | 
  5 | import logging
  6 | import time
  7 | import numpy as np
  8 | import traceback
  9 | import math
 10 | from datetime import datetime
 11 | import os
 12 | import walk_conf as wconf
 13 | from tf_walk import TFWalkClassifier
 14 | import cv2 as cv
 15 | 
 16 | MAX_STEP_ATTEMPT = 2
 17 | 
 18 | class WalkCtrl:
 19 | 
 20 |     motor_ctrl = None
 21 |     photo_ctrl = None
 22 | 
 23 |     max_iter = 100
 24 | 
 25 |     last_act = 0
 26 |     last_angle = 0 
 27 | 
 28 |     photo_n = 0
 29 | 
 30 |     path = None
 31 |     g_path = None
 32 |     s_path = None
 33 |     p_path = None
 34 |     last_phid = None
 35 | 
 36 |     last_g_path = None
 37 |     last_s_path = None
 38 |     last_p_path = None
 39 | 
 40 |     iter_n = 0
 41 | 
 42 |     track_id = None
 43 |     last_photo_path = None
 44 | 
 45 |     tfc = None
 46 | 
 47 |     avg = 0
 48 |     prepare = False
 49 |     last_action = None
 50 |     step_attempt = 0
 51 | 
 52 |     def next(self):
 53 |         self.iter_n += 1
 54 |         return self.iter_n < self.max_iter
 55 | 
 56 | 
 57 |     def __init__(self, motor, photo):
 58 |         self.motor_ctrl = motor
 59 |         self.photo_ctrl = photo
 60 | 
 61 |         self.tfc = TFWalkClassifier()
 62 |         logging.debug("Initializing TF...")
 63 |         self.tfc.init()
 64 |         logging.debug("TF inited")
 65 |                                                                 
 66 | 
 67 | 
 68 | 
 69 |     def init(self):
 70 |         self.photo_n = 0
 71 |         self.iter_n = 0
 72 |         self.track_id = self.get_id()
 73 |         self.path = PiConf.PHOTO_PATH + "/walk/" + self.track_id
 74 |         if not os.path.isdir(self.path):
 75 |             os.makedirs(self.path)
 76 |         return self.track_id
 77 | 
 78 | 
 79 | 
 80 |     def follow(self):
 81 |         logging.debug("Start walk following")
 82 |         if not self.track_id:
 83 |             self.init()
 84 | 
 85 |         self.motor_ctrl.set_motors("f", "f")   
 86 |         self.last_act = -1
 87 | 
 88 |         try:
 89 |             while(self.next()):
 90 |                 if not self.follow_step(self.iter_n):
 91 |                     break
 92 |         except Exception as e:
 93 |             logging.exception("Cannot do a walk step")
 94 |         finally:
 95 |             self.motor_ctrl.set_motors("s", "s")
 96 | 
 97 |         logging.debug("Done walk following")
 98 | 
 99 |         self.path = None
100 |         self.track_id = None
101 |         self.photo_n = 0
102 |         self.iter_n = 0
103 | 
104 | 
105 |     def follow_step(self, i):
106 |         logging.debug(("Follow walk step", i))
107 |         a = self.get_action()
108 | 
109 | 
110 |         if a is None:
111 |             logging.debug(("No action from pic", self.last_s_path, "last action", self.last_action, "attempt", self.step_attempt))
112 |             self.last_p_path = None
113 |             if self.step_attempt > MAX_STEP_ATTEMPT or self.last_action is None:
114 |                 return False
115 |             self.step_attempt += 1
116 |             a = self.last_action
117 |         else:
118 |             self.last_action = a
119 |             self.step_attempt = 0
120 | 
121 |         self.last_action = a
122 |     
123 |         self.draw_action_pic(a)
124 | 
125 |         turn_val = 0.15
126 |         straight_run = 0.5
127 | 
128 |         if a != 0:
129 |             self.turn(a, turn_val)
130 |         else:
131 |             time.sleep(straight_run)
132 |         self.last_act = a
133 |         return True
134 | 
135 | 
136 | 
137 |     def get_photo(self):
138 |         logging.debug("Make walk photo")
139 |         rc, phid = self.photo_ctrl.make_photo()
140 |         if rc  == False:
141 |             return False, None, None
142 |         fpath, fname = self.photo_ctrl.get_path(phid)
143 |         if fpath is None:
144 |             return False, None, None
145 |         self.photo_n += 1
146 |         return True, phid, fpath + "/" + fname
147 | 
148 |     def get_photo_paths(self, fname):
149 |         return PiConf.PHOTO_PATH + "/" + fname  + "-s.jpg", PiConf.PHOTO_PATH + "/" + fname  + "-g.jpg"
150 | 
151 | 
152 |     def draw_action_pic(self,a):
153 |         p_path = PiConf.PHOTO_PATH + "/" + self.last_phid + "-p.jpg"
154 |         return self._draw_action_pic(a,p_path)
155 | 
156 |     def _draw_action_pic(self,a, p_path):
157 |         word = "S"
158 |         if a < 0:
159 |             word = "L"
160 |         elif a > 0:
161 |             word = "R"
162 | 
163 |         rc = wconf.apply_mask_word(self.last_s_path, self.last_g_path, p_path, (0, 0, 255), word, (255, 0, 0))
164 |         if rc:
165 |             self.last_p_path = p_path
166 |             logging.debug(("Saved action pic", self.last_p_path))
167 | 
168 |         return rc
169 | 
170 | 
171 | 
172 |     def prepare_action(self):
173 |         self.prepare = True
174 |         logging.debug("Prepare walk")
175 |         a = self.get_action()
176 |         if a is None:
177 |             print("No action from pic", self.last_s_path)
178 |             logging.debug(("No action from pic", self.last_s_path))
179 |             self.last_p_path = None
180 |             return False
181 |         logging.debug(("Prepare action", a, self.last_s_path, self.last_g_path))
182 |         rc = self.draw_action_pic(a)
183 |         self.prepare = False
184 |         self.step_attempt = 0
185 |         self.last_action = None
186 |         return rc
187 | 
188 |     def get_action_pic(self):
189 |         ret = self.last_p_path if self.last_p_path is not None else self.last_s_path
190 |         logging.debug(("Last walk pic", ret, self.last_p_path, self.last_s_path))
191 |         return None if ret is None else ret[len(PiConf.PHOTO_PATH) + 1:-4]
192 | 
193 | 
194 |     def handle_small(self, small, g_path):
195 |         hsv = wconf.to_hsv(small)
196 |         bright = wconf.get_bright(hsv)
197 |         logging.debug(("Saved walk small photo", self.last_s_path, "bright", bright))
198 | 
199 |         avg = int(bright * 1.1)
200 |         if self.prepare or self.avg < 1:
201 |             logging.debug(("Set avg bright", avg))
202 |             self.avg = avg
203 |             ar, ag, ab = wconf.get_avg_rgb(small, 8)
204 |             logging.debug(("Rgb rect", ar, ag, ab))
205 |         else:
206 |             avg = self.avg
207 |             logging.debug(("Use prepared avg", avg))
208 | 
209 |         gray_EPS = wconf.gray_EPS
210 |         rgb_EPS = wconf.rgb_EPS
211 |         blur = wconf.gray_blur
212 | 
213 |         gray = wconf.filter_gray(small, gray_EPS, avg, rgb_EPS, blur, g_path)
214 |         self.last_g_path = g_path
215 |         wp = wconf.get_white_percent(gray)
216 |         logging.debug(("Saved walk gray photo", g_path, "avg", avg, "white", wp))
217 |         if wp < wconf.white_threshold:
218 |             logging.debug(("Gray image is all black", wp, avg))
219 |             return None
220 | 
221 |         #gray = cv.imread(g_path, cv.IMREAD_GRAYSCALE)
222 |         logging.debug(("Classifying", g_path))
223 |         c = self.tfc.classify(gray)
224 |         logging.debug(("Classified as", c))
225 |         if c is None:
226 |             return None
227 |         ret = 0
228 |         if c == TFWalkClassifier.TF_LEFT:
229 |             ret = -1
230 |         if c == TFWalkClassifier.TF_RIGHT:
231 |             ret = 1
232 |         
233 |         return ret
234 | 
235 | 
236 |     def get_action(self):
237 | 
238 |         rc, phid, fpath = self.get_photo()
239 |         if not rc:
240 |             logging.debug(("Cannot get a photo for walk"))
241 |             return None
242 | 
243 |         #phid = "31012019215149-624507"
244 |         #fpath = PiConf.PHOTO_PATH + "/" + phid + ".jpg"
245 |         self.last_phid = phid
246 | 
247 |                 
248 |         s_path, g_path = self.get_photo_paths(phid)
249 |         
250 |         size = wconf.resize_size
251 |         blur = wconf.resize_blur
252 |         img, small = wconf.blur_resize(fpath, s_path, blur, size)
253 |         if img is None:
254 |             logging.debug(("Cannot load photo", fpath))
255 |             return None
256 | 
257 |         self.last_s_path = s_path
258 | 
259 |         return self.handle_small(small, g_path)
260 | 
261 |     def turn(self, r, t):
262 |         turn_cmd = "s0" if r > 0 else "0s"
263 |         ret_cmd = "f0" if r > 0 else "0f"
264 |         turn = "Right" if r > 0 else "Left"
265 |         logging.debug(("Turn", turn, t))
266 |         self.motor_ctrl.set_motors(turn_cmd[0], turn_cmd[1])
267 |         time.sleep(t)
268 |         self.motor_ctrl.set_motors(ret_cmd[0], ret_cmd[1])
269 | 
270 | 
271 |     def get_id(self):
272 |         return datetime.now().strftime('%d%m%Y%H%M%S')
273 | 
274 | 
275 | def createWalkCtrl(motor_ctrl, photo_ctrl):
276 |     return WalkCtrl(motor_ctrl, photo_ctrl)
277 |             
278 | 
279 | if __name__ == '__main__':
280 | 
281 |     log_file = "/home/pi/test.log"
282 |     logging.basicConfig(filename=log_file,level=logging.DEBUG, format='%(asctime)s.%(msecs)03d %(threadName)s %(levelname)-8s %(message)s', datefmt='%Y-%m-%d %H:%M:%S')
283 | 
284 | 
285 |     motor_ctrl = MotorCtrl.createMotorCtrl()
286 |     photo_ctrl = PhotoCtrl.createPhotoCtrl()
287 | 
288 |     f = WalkCtrl(motor_ctrl, photo_ctrl)
289 |     #f.avg = 150
290 |     s_path = "photos/nn-s.jpg"    
291 |     f.last_s_path = s_path
292 | 
293 |     img = cv.imread(s_path)
294 |     g_path = "test/data/g_nn.jpg"    
295 |     a = f.handle_small(img, g_path)
296 |     print ("Predict", a)


--------------------------------------------------------------------------------
/PCA9685.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/python
  2 | '''
  3 | **********************************************************************
  4 | * Filename    : PCA9685.py
  5 | * Description : A driver module for PCA9685
  6 | * Author      : Cavon
  7 | * Brand       : SunFounder
  8 | * E-mail      : service@sunfounder.com
  9 | * Website     : www.sunfounder.com
 10 | * Version     : v1.2.0
 11 | **********************************************************************
 12 | '''
 13 | 
 14 | import smbus
 15 | import time
 16 | import math
 17 | 
 18 | class PWM(object):
 19 |     """A PWM control class for PCA9685."""
 20 |     _MODE1              = 0x00
 21 |     _MODE2              = 0x01
 22 |     _SUBADR1            = 0x02
 23 |     _SUBADR2            = 0x03
 24 |     _SUBADR3            = 0x04
 25 |     _PRESCALE           = 0xFE
 26 |     _LED0_ON_L          = 0x06
 27 |     _LED0_ON_H          = 0x07
 28 |     _LED0_OFF_L         = 0x08
 29 |     _LED0_OFF_H         = 0x09
 30 |     _ALL_LED_ON_L       = 0xFA
 31 |     _ALL_LED_ON_H       = 0xFB
 32 |     _ALL_LED_OFF_L      = 0xFC
 33 |     _ALL_LED_OFF_H      = 0xFD
 34 | 
 35 |     _RESTART            = 0x80
 36 |     _SLEEP              = 0x10
 37 |     _ALLCALL            = 0x01
 38 |     _INVRT              = 0x10
 39 |     _OUTDRV             = 0x04
 40 | 
 41 |     RPI_REVISION_0 = ["900092"]
 42 |     RPI_REVISION_1_MODULE_B = ["Beta", "0002", "0003", "0004", "0005", "0006", "000d", "000e", "000f"]
 43 |     RPI_REVISION_1_MODULE_A = ["0007", "0008", "0009",]
 44 |     RPI_REVISION_1_MODULE_BP = ["0010", "0013"]
 45 |     RPI_REVISION_1_MODULE_AP = ["0012"]
 46 |     RPI_REVISION_2 = ["a01041", "a21041"]
 47 |     RPI_REVISION_3 = ["a02082", "a22082"]
 48 | 
 49 |     _DEBUG = False
 50 |     _DEBUG_INFO = 'DEBUG "PCA9685.py":'
 51 | 
 52 |     def _get_bus_number(self):
 53 |         pi_revision = self._get_pi_revision()
 54 |         if   pi_revision == '0':
 55 |             return 0
 56 |         elif pi_revision == '1 Module B':
 57 |             return 0
 58 |         elif pi_revision == '1 Module A':
 59 |             return 0
 60 |         elif pi_revision == '1 Module B+':
 61 |             return 1
 62 |         elif pi_revision == '1 Module A+':
 63 |             return 0
 64 |         elif pi_revision == '2 Module B':
 65 |             return 1
 66 |         elif pi_revision == '3 Module B':
 67 |             return 1
 68 | 
 69 |     def _get_pi_revision(self):
 70 |         "Gets the version number of the Raspberry Pi board"
 71 |         # Courtesy quick2wire-python-api
 72 |         # https://github.com/quick2wire/quick2wire-python-api
 73 |         # Updated revision info from: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
 74 |         try:
 75 |             f = open('/proc/cpuinfo','r')
 76 |             for line in f:
 77 |                 if line.startswith('Revision'):
 78 |                     if line[11:-1] in self.RPI_REVISION_0:
 79 |                         return '0'
 80 |                     elif line[11:-1] in self.RPI_REVISION_1_MODULE_B:
 81 |                         return '1 Module B'
 82 |                     elif line[11:-1] in self.RPI_REVISION_1_MODULE_A:
 83 |                         return '1 Module A'
 84 |                     elif line[11:-1] in self.RPI_REVISION_1_MODULE_BP:
 85 |                         return '1 Module B+'
 86 |                     elif line[11:-1] in self.RPI_REVISION_1_MODULE_AP:
 87 |                         return '1 Module A+'
 88 |                     elif line[11:-1] in self.RPI_REVISION_2:
 89 |                         return '2 Module B'
 90 |                     elif line[11:-1] in self.RPI_REVISION_3:
 91 |                         return '3 Module B'
 92 |                     else:
 93 |                         print ("Error. Pi revision didn't recognize, module number: %s" % line[11:-1])
 94 |                         print ('Exiting...')
 95 |                         quit()
 96 |         except Exception as e:
 97 |             f.close()
 98 |             print (e)
 99 |             print ('Exiting...')
100 |             quit()
101 |         finally:
102 |             f.close()
103 | 
104 |     def __init__(self, bus_number=None, address=0x40):
105 |         '''Init the class with bus_number and address'''
106 |         if self._DEBUG:
107 |             print (self._DEBUG_INFO, "Debug on")
108 |         self.address = address
109 |         if bus_number == None:
110 |             self.bus_number = self._get_bus_number()
111 |         else:
112 |             self.bus_number = bus_number
113 |         self.bus = smbus.SMBus(self.bus_number)
114 |         if self._DEBUG:
115 |             print (self._DEBUG_INFO, 'Reseting PCA9685 MODE1 (without SLEEP) and MODE2')
116 |         self.write_all_value(0, 0)
117 |         self._write_byte_data(self._MODE2, self._OUTDRV)
118 |         self._write_byte_data(self._MODE1, self._ALLCALL)
119 |         time.sleep(0.005)
120 | 
121 |         mode1 = self._read_byte_data(self._MODE1)
122 |         mode1 = mode1 & ~self._SLEEP
123 |         self._write_byte_data(self._MODE1, mode1)
124 |         time.sleep(0.005)
125 |         self.frequency = 60
126 | 
127 |     def _write_byte_data(self, reg, value):
128 |         '''Write data to I2C with self.address'''
129 |         if self._DEBUG:
130 |             print (self._DEBUG_INFO, 'Writing value %2X to %2X' % (value, reg))
131 |         #try:
132 |         self.bus.write_byte_data(self.address, reg, value)
133 |         #except Exception, i:
134 |         #    print i
135 |         #    self._check_i2c()
136 | 
137 |     def _read_byte_data(self, reg):
138 |         '''Read data from I2C with self.address'''
139 |         if self._DEBUG:
140 |             print (self._DEBUG_INFO, 'Reading value from %2X' % reg)
141 |         try:
142 |             results = self.bus.read_byte_data(self.address, reg)
143 |             return results
144 |         except Exception as i:
145 |             print (i)
146 |             self._check_i2c()
147 | 
148 |     def _check_i2c(self):
149 |         import commands
150 |         bus_number = self._get_bus_number()
151 |         print ("\nYour Pi Rivision is: %s" % self._get_pi_revision())
152 |         print ("I2C bus number is: %s" % bus_number)
153 |         print ("Checking I2C device:")
154 |         cmd = "ls /dev/i2c-%d" % bus_number
155 |         output = commands.getoutput(cmd)
156 |         print ('Commands "%s" output:' % cmd)
157 |         print (output)
158 |         if '/dev/i2c-%d' % bus_number in output.split(' '):
159 |             print ("I2C device setup OK")
160 |         else:
161 |             print ("Seems like I2C has not been set. Use 'sudo raspi-config' to set I2C")
162 |         cmd = "i2cdetect -y %s" % self.bus_number
163 |         output = commands.getoutput(cmd)
164 |         print ("Your PCA9685 address is set to 0x%02X" % self.address)
165 |         print ("i2cdetect output:")
166 |         print (output)
167 |         outputs = output.split('\n')[1:]
168 |         addresses = []
169 |         for tmp_addresses in outputs:
170 |             tmp_addresses = tmp_addresses.split(':')[1]
171 |             tmp_addresses = tmp_addresses.strip().split(' ')
172 |             for address in tmp_addresses:
173 |                 if address != '--':
174 |                     addresses.append(address)
175 |         print ("Conneceted i2c device:")
176 |         if addresses == []:
177 |             print ("None")
178 |         else:
179 |             for address in addresses:
180 |                 print ("  0x%s" % address)
181 |         if "%02X" % self.address in addresses:
182 |             print ("Wierd, I2C device is connected. Try to run the program again. If the problem's still, email the error message to service@sunfounder.com")
183 |         else:
184 |             print ("Device is missing.")
185 |             print ("Check the address or wiring of PCA9685 servo driver, or email the error message to service@sunfounder.com")
186 |             #print 'Exiting...'
187 |         #quit()
188 | 
189 |     @property
190 |     def frequency(self):
191 |         return _frequency
192 | 
193 |     @frequency.setter
194 |     def frequency(self, freq):
195 |         '''Set PWM frequency'''
196 |         if self._DEBUG:
197 |             print (self._DEBUG_INFO, 'Set frequency to %d' % freq)
198 |         self._frequency = freq
199 |         prescale_value = 25000000.0
200 |         prescale_value /= 4096.0
201 |         prescale_value /= float(freq)
202 |         prescale_value -= 1.0
203 |         if self._DEBUG:
204 |             print (self._DEBUG_INFO, 'Setting PWM frequency to %d Hz' % freq)
205 |             print (self._DEBUG_INFO, 'Estimated pre-scale: %d' % prescale_value)
206 |         prescale = math.floor(prescale_value + 0.5)
207 |         if self._DEBUG:
208 |             print (self._DEBUG_INFO, 'Final pre-scale: %d' % prescale)
209 | 
210 |         old_mode = self._read_byte_data(self._MODE1);
211 |         new_mode = (old_mode & 0x7F) | 0x10
212 |         self._write_byte_data(self._MODE1, new_mode)
213 |         self._write_byte_data(self._PRESCALE, int(math.floor(prescale)))
214 |         self._write_byte_data(self._MODE1, old_mode)
215 |         time.sleep(0.005)
216 |         self._write_byte_data(self._MODE1, old_mode | 0x80)
217 | 
218 |     def write(self, channel, on, off):
219 |         '''Set on and off value on specific channel'''
220 |         if self._DEBUG:
221 |             print (self._DEBUG_INFO, 'Set channel "%d" to value "%d"' % (channel, off))
222 |         self._write_byte_data(self._LED0_ON_L+4*channel, on & 0xFF)
223 |         self._write_byte_data(self._LED0_ON_H+4*channel, on >> 8)
224 |         self._write_byte_data(self._LED0_OFF_L+4*channel, off & 0xFF)
225 |         self._write_byte_data(self._LED0_OFF_H+4*channel, off >> 8)
226 | 
227 |     def write_all_value(self, on, off):
228 |         '''Set on and off value on all channel'''
229 |         if self._DEBUG:
230 |             print (self._DEBUG_INFO, 'Set all channel to value "%d"' % (off))
231 |         self._write_byte_data(self._ALL_LED_ON_L, on & 0xFF)
232 |         self._write_byte_data(self._ALL_LED_ON_H, on >> 8)
233 |         self._write_byte_data(self._ALL_LED_OFF_L, off & 0xFF)
234 |         self._write_byte_data(self._ALL_LED_OFF_H, off >> 8)
235 | 
236 |     def map(self, x, in_min, in_max, out_min, out_max):
237 |         '''To map the value from arange to another'''
238 |         return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
239 | 
240 |     @property
241 |     def debug(self):
242 |         return self._DEBUG
243 | 
244 |     @debug.setter
245 |     def debug(self, debug):
246 |         '''Set if debug information shows'''
247 |         if debug in (True, False):
248 |             self._DEBUG = debug
249 |         else:
250 |             raise ValueError('debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'.format(debug))
251 | 
252 |         if self._DEBUG:
253 |             print (self._DEBUG_INFO, "Set debug on")
254 |         else:
255 |             print (self._DEBUG_INFO, "Set debug off")
256 | 
257 | if __name__ == '__main__':
258 |     import time
259 | 
260 |     pwm = PWM()
261 |     pwm.frequency = 60
262 |     for i in range(16):
263 |         time.sleep(0.5)
264 |         print ('\nChannel %d\n' % i)
265 |         time.sleep(0.5)
266 |         for j in range(4096):
267 |             pwm.write(i, 0, j)
268 |             print ('PWM value: %d' % j)
269 |             time.sleep(0.0003)
270 | 


--------------------------------------------------------------------------------