def _followByDistance(self): """ internal function for followDistance mode follow with Ultrasonic by keeping the same distance to target this function leverage _distanceScanWorker to stop """ maxDistance = self.config.getOrAddFloat('follow.maxFollowDistance', 2.0) dis = self.distance if dis < maxDistance: #Check if the target is in diatance range distanceToFollow = self.config.getOrAddFloat('distanceScan.stopDistanceInMeter', 0.2) # keep the distance to the target distanceOffset = self.config.getOrAddFloat('follow.distanceOffset', 0.1) # controls the sensitivity if dis > (distanceToFollow + distanceOffset) : #If the target is in distance range and out of distanceToFollow, then move forward if self.drive.motor.speed > 0: pass else: self.drive.setLedsRGB(picarConst.CYAN) self.drive.forward(self.config.getOrAddInt('auto.forwardSpeed', 60)) timePrint('followByDistance - move forward. distance: %s' %dis) elif dis < (distanceToFollow - distanceOffset) : #Check if the target is too close, if so, the car move back to keep distance at distance if self.drive.motor.speed < 0: pass else: self.drive.setLedsRGB(picarConst.PINK) self.drive.backward(self.config.getOrAddInt('auto.backwardSpeed', 60)) timePrint('followByDistance - move backward. distance: %s' %dis) else: #If the target is at distance, then the car stay still self.drive.setLedsRGB(picarConst.GREEN) if abs(self.drive.motor.speed) > 5: self.drive.stopMotor() else: if abs(self.drive.motor.speed) > 5: self.drive.stopMotor()
def _distanceScanWorker(self): """ internal thread for measuring distance at specified interval """ interval = self.config.getOrAddFloat('distanceScan.scanCycleInSecond', 0.2) # delay interval for the worker stopDistance = self.config.getOrAddFloat('distanceScan.stopDistanceInMeter', 0.2) # the distance to stop forward movement slowDistance = self.config.getOrAddFloat('distanceScan.slowDistanceInMeter', 1.0) # the distance to stop forward movement headingAngleLimit = self.config.getOrAddInt('distanceScan.headingAngleLimit', 20) # the angle limit considered as measuring straight ahead while True: slowdown = 0 if self.distanceScan and not self.head.scanning: distance = self.head.ultrasonic.pingDistance() if distance < 0: # give it one more try distance = self.head.ultrasonic.pingDistance() self.distance = distance # check distance to stop drive if stopDistance > 0 and distance > 0: # check heading and forward (speed > 0) before stopping hAngle, vAngle = self.head.heading if abs(hAngle) < headingAngleLimit and abs(vAngle) < headingAngleLimit and self.drive.motor.speed > 0: if distance < stopDistance: timePrint('Stopping drive at distance: %f' %distance) self.drive.stop() slowdown=0 elif distance < slowDistance: slowdown = -int(30 * (slowDistance - distance) / (slowDistance - stopDistance)) #timePrint('Slowing down %i at distance: %f' %(slowdown, distance)) self.drive.extraSpeed(slowdown) time.sleep(interval)
def doPost(self, request): """ handle POST request for Pi system resources request: the RequestContext data object must return a Flask.Response object """ bodyJson = request.json response = [] httpStatusCode = 200 for key, value in bodyJson.items(): try: timePrint('POST system command: %s=%s' %(key, value)) # use tempfile for output tempFile = tempfile.NamedTemporaryFile(prefix='pisys-') tempFileName = tempFile.name tempFile.close() command = '%s > %s 2>&1 \n' %(value, tempFileName) os.system(command) f = open(tempFileName, "r") text = f.read() f.close() os.remove(tempFileName) itemStatusCode = 200 itemResponse = { key: value, 'result': text } except: itemStatusCode = 400 itemResponse = { KeyResponse: 'InvalidCommand', KeyPropertyValue: value } if itemStatusCode != 200: httpStatusCode = itemStatusCode response.append(itemResponse) return makeJsonResponse(httpStatusCode, response)
def setOperationMode(self, mode): """ set PiCar's operation mode """ if mode >= picarConst.ManualMode and mode <= picarConst.FaceTrackingMode: self.mode = mode return True else: timePrint('Invalid operation mode: %i' %mode) return False
def wait(self): """Invoked from each client's thread to wait for the next frame.""" ident = get_ident() if ident not in self.events: # this is a new client # add an entry for it in the self.events dict # each entry has two elements, a threading.Event() and a timestamp timePrint('add camera event with thread id: %s' % ident) self.events[ident] = [threading.Event(), time.time()] return self.events[ident][0].wait()
def __init__(self, width=1280, height=720, crosshair=False): """ initialize a PiCam with specified width and height """ Camera.width = width Camera.height = height self.cameraReady = False try: with picamera.PiCamera() as camera: self.cameraReady = True super(Camera, self).__init__(crosshair=crosshair) except: timePrint("Failed to initialize picamera")
def scan(self, starth, startv, endh, endv, inch, incv, delay=0.2): """ perform distance scan with inputs starth, startv - start position for horizontal and vertical endh, endv - end position for horizontal and vertical inch, incv - increment position for horizontal and vertical delay - delay time between each scan point returns arrays of the followings: - array of distance values - array of horizonal positions - array of vertical positions """ hOriginalPos = self.servoH.angle vOriginalPos = self.servoV.angle self.scanning = True timePrint('Scan starth ' + str(starth) + ' startv ' + str(startv) + ' endh ' + str(endh) + ' endv ' + str(endv) + ' inch ' + str(inch) + ' incv ' + str(incv)) values = [] hValues = [] vValues = [] hPos = starth vPos = startv self.servoH.moveToAngle(hPos) # move to horizontal start position self.servoV.moveToAngle(vPos) # move to vertical start position time.sleep(delay) #Wait for the Ultrasonic to be in position while hPos < endh or vPos < endv: #Scan,from start to end #timePrint('scan hpos: ' + str(hPos) + ' vpos: ' + str(vPos)) new_scan_data = round(self.ultrasonic.pingDistance(), 2) #Get a distance of a certern direction values.append( new_scan_data ) #Put that distance value into a list,and save it as String-Type for future transmission hValues.append(hPos) vValues.append(vPos) hPosNew = hPos + inch if hPosNew > endh: hPosNew = endh vPosNew = vPos + incv if vPosNew > endv: vPosNew = endv if hPos != hPosNew: hPos = hPosNew self.servoH.moveToAngle(hPos) if vPos != vPosNew: vPos = vPosNew self.servoV.moveToAngle(vPos) time.sleep(delay) # move back to original positions self.servoH.moveToAngle(hOriginalPos) self.servoV.moveToAngle(vOriginalPos) self.scanning = False return [values, hValues, vValues]
def doPost(self, request): """ handle POST request for Pi system resources request: the RequestContext data object must return a Flask.Response object """ component = request.paths[1] bodyDict = request.json response = [] httpStatusCode = 200 try: timePrint('Post to picar: ' + str(bodyDict)) if 'scan' == component: # for scan, the bodyDict contains strt end and inc for the scan response = self.commandHandler.doScanCommand(bodyDict) httpStatusCode = response.pop(KeyStatusCode, 400) elif 'settings' in component: config = self.commandHandler.car.config oldAutoSave = config.autoSave config.autoSave = False for key, value in bodyDict.items(): config.set(key, value) config.save(forceSave=True) config.autoSave = oldAutoSave response = {KeyResponse: 'Settings saved '} else: # process all the values in the body httpStatusCode = 200 comma = '' for key, value in bodyDict.items(): fullPath = request.path + '/' + key itemResponse = self.commandHandler.doCommand( fullPath, value) #timePrint(response) statusCode = itemResponse.pop(KeyStatusCode, 400) if statusCode != 200: httpStatusCode = statusCode response.append(itemResponse) except Exception as e: print('Exception handling request: ' + str(e)) traceback.print_exc() httpStatusCode = 500 response = {KeyResponse: 'Exception: ' + str(e)} return makeJsonResponse(httpStatusCode, response)
def set(self): """Invoked by the camera thread when a new frame is available.""" now = time.time() remove = None for ident, event in self.events.items(): if not event[0].isSet(): # if this client's event is not set, then set it # also update the last set timestamp to now event[0].set() event[1] = now else: # if the client's event is already set, it means the client # did not process a previous frame # if the event stays set for more than 5 seconds, then assume # the client is gone and remove it if now - event[1] > 5: remove = ident if remove: timePrint('remove camera event with thread id: %s' % remove) del self.events[remove]
def _faceTracking(self): """ tracking a face by moving head to follow the face """ faceTracker = self.head.camera.faceTracker trackedFaces = faceTracker.getTrackedFaces() if len(trackedFaces) == 0: if self._faceId < 0: # todo: searching faces by looking left/right pass return if self._faceId in trackedFaces: faceTrackingData = trackedFaces[self._faceId] #elif self._faceId < 0: else: # get the first face tracked self._faceId = next(iter(trackedFaces)) faceTrackingData = trackedFaces[self._faceId] timePrint('Start tracking face ID %i' %self._faceId) # lost the tracked face #timePrint('Lost tracked face ID %i' %self._faceId) #return # find the center of the tracked face x, y, w, h = faceTrackingData.getPosition() x = int(x + w / 2) y = int(y + h / 2) # center of the image imageHeight, imageWidth, c = faceTracker.getImageShape() xc = int(imageWidth / 2) yc = int(imageHeight / 2) # calculate angles to move xd = int(((x - xc) / imageWidth) * self.config.getOrAddInt('faceTracking.horizontalViewAngle', 54)) yd = int(((yc - y) / imageHeight) * self.config.getOrAddInt('faceTracking.verticalViewAngle', 42)) #timePrint('x: %i y: %i xc: %i yc: %i xd: %i yd: %i' %(x, y, xc, yc, xd, yd)) xAngle, yAngle = self.head.heading if abs(xd) > 5: angle = xAngle + xd #timePrint('Move head horizontal by %i degree to %i degree' %(xd, angle)) self.head.moveHorizontal(angle) if abs(yd) > 5: angle = yAngle + yd #timePrint('Move head vertical by %i degree to %i degree' %(yd, angle)) self.head.moveVertical(angle)
def _commandToSpeechAsync(self, commandStatus): """ send commandStatus to CommandToSpeech service """ speechMode = self.config.getOrAdd('piCar.commandToSpeechMode', 'off') if 'on' in speechMode.lower() and self.commandToSpeechErrorCount < 3: if self.commandToSpeechAll or commandStatus.lower() in self.speechList: speakService = self.config.get('piCar.commandToSpeechService') credential = self.config.get('piCar.commandToSpeechCredential') auth = None if len(credential) > 0: user, password = credential.split(':') auth=(user, password) try: r = requests.post(speakService, data = json.dumps({'text': commandStatus}), auth=auth) timePrint('Send command to speech %s' %commandStatus) self.commandToSpeechErrorCount = 0 except Exception as e: print('Exception command to speech %s: %s' %(commandStatus, str(e))) self.commandToSpeechErrorCount += 1 else: # clear error count if commandToSpeechMode is off self.commandToSpeechErrorCount = 0
def _thread(cls): """Camera background thread.""" timePrint('Starting camera thread %s' % get_ident()) frames_iterator = cls.frames() for frame in frames_iterator: BaseCamera.frame = frame if BaseCamera.faceTracker != None: BaseCamera.trackingFrame = frame.copy() BaseCamera.faceTracker.detectOrTrack(BaseCamera.trackingFrame) BaseCamera.event.set() # send signal to clients time.sleep(0) # if there hasn't been any clients asking for frames in # the last 10 seconds then stop the thread if time.time() - BaseCamera.last_access > 10: frames_iterator.close() timePrint('Stopping camera thread due to inactivity %s' % get_ident()) break BaseCamera.thread = None BaseCamera.faceTracker = None
def tcpVideoWorker(car, videoSocket, tracking=True): # OpenCV and FPV video """ control the picar to follow an object """ #car.tcpVideo(car, videoSocket, tracking=True) font = cv2.FONT_HERSHEY_SIMPLEX timePrint('Started TCP Video') time.sleep(1) picam = car.head.camera picam.camera.start(faceTracker=picam.faceTracker) while True: image = picam.camera.get_frame(tracking=tracking) #timePrint('processing image') cv2.line(image, (300, 240), (340, 240), (128, 255, 128), 1) cv2.line(image, (320, 220), (320, 260), (128, 255, 128), 1) dis = car.distance if dis < 8: cv2.putText(image, '%s m' % str(round(dis, 2)), (40, 40), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) encoded, buffer = cv2.imencode('.jpg', image) jpg_as_text = base64.b64encode(buffer) videoSocket.send(jpg_as_text)
def doScanCommand(self, bodyDict): """ execute the scan command specified by the bodyDict bodyDict is a dict for the request body (the json payload) returns a dictionary of scan result - statusCode - value: an array of distance values - posh: an array of horizontal position - posv: an array of vrtical position """ self.commandToSpeech('Start Scan') value = [] posh = [] posv = [] starth = int(getFromDict(bodyDict, 'starth', -90)) startv = int(getFromDict(bodyDict, 'startv', 0)) endh = int(getFromDict(bodyDict, 'endh', 90)) endv = int(getFromDict(bodyDict, 'endv', 0)) inch = int(getFromDict(bodyDict, 'inch', 2)) incv = int(getFromDict(bodyDict, 'incv', 2)) value, posh, posv = self.head.scan(starth, startv, endh, endv, inch, incv) timePrint('scan completed with ' + str(len(value)) + ' values') response = {'statusCode': 200, 'response': 'scanResults', 'value': value, 'posh': posh, 'posv': posv} return response
def processRequest(cls, request): """ process a request by finding the request handler """ method = request.method key = request.rootPath.lower() if key in BaseRequestHandler._handlers: handler = BaseRequestHandler._handlers[key] timePrint('%s %s client: %s handler: %s' % (method, request.url, request.remote_addr, handler.name)) try: if handler.checkAuth(request): if method == 'GET': response = handler.doGet(request) elif method == 'POST': response = handler.doPost(request) elif method == 'PUT': response = handler.doPut(request) elif method == 'DELETE': response = handler.doDelete(request) else: response = makeJsonResponse( 403, {'response': 'InvalidMethod'}) else: response = makeJsonResponse(401, {'response': 'UnAuthorized'}) except Exception as e: timePrint('Exception handling request: ' + str(e)) traceback.print_exc() response = makeJsonResponse( 500, {'response': 'Exception: ' + str(e)}) elif len(key) == 0: # send available discovery paths if rootPath is not specified result = [] for key in BaseRequestHandler._handlers: handler = BaseRequestHandler._handlers[key] #result.append({'name': handler.name, 'path': key, 'url': request.host_url + key}) for item in handler.endpoints(key, request.host_url): result.append(item) response = makeJsonResponse(200, result) else: timePrint('%s %s client: %s handler: N.A.' % (method, request.url, request.remote_addr)) response = makeJsonResponse(403, {'response': 'InvalidPath'}) timePrint( '%s %s client: %s status: %s' % (method, request.url, request.remote_addr, response.status_code)) return response
def __init__(self, name, type, rootPaths, authorizationList=None): """ constructor for BaseRequestHandler name: the name for the handler type: the type of the handler rootPaths: list of root paths that will be handled by the instance authorizationList: a list of strings for authorized users """ self.name = name self.type = type self.rootPaths = rootPaths if authorizationList == None or len(authorizationList) == 0: self.authorizationList = None else: self.authorizationList = authorizationList # register the handler with BaseRequestHandler._lock: for key in rootPaths: if key in BaseRequestHandler._handlers: raise Exception( 'RequestHandler with the same rootPath: %s' % key) BaseRequestHandler._handlers[key.lower()] = self timePrint('Registered RequestHandler: %s rootPaths: %s' % (self.name, str(rootPaths)))
def detectOrTrack(self, img): """ this is the main function for FaceTracker to track the faces inside images detectOrTrack returns the image with boxes around all faces tracked """ # 1. update all trackers and remove the ones with lower quality (defined by trackQualityBar) # 2. runs face detection for every framesForDetection frames # - for each face try to match an existing tracked faces: # . the face's centerpoint is inside existing tracked box # . the centerpoint of the tracked box is also inside the face's box # - if no match add a new tracker with a new face-id fidsToDelete = [] for fid in self.trackedFaces.keys(): trackingQuality = self.trackedFaces[fid].update(img) if trackingQuality < self.trackQualityLowBar: timePrint("Removing face ID " + str(fid) + ' quality: ' + str(trackingQuality)) fidsToDelete.append(fid) for fid in fidsToDelete: self.trackedFaces.pop(fid, None) # determine whether to run face detection if (self.frameCounter % self.framesForDetection) == 0: faces = self.detectFaces(img) #if self.debugMode and len(faces) != len(self.trackedFaces): #timePrint('Found faces: ' + str(len(faces)) + ' Tracking faces: ' + str(len(self.trackedFaces))) for (_x, _y, _w, _h) in faces: # convert to int since dlib requires numpy.int32 x = int(_x) y = int(_y) w = int(_w) h = int(_h) #calculate the centerpoint x_center = x + 0.5 * w y_center = y + 0.5 * h matchedFid = None # go thru all the trackers and check if the centerpoint of the face is within the box of a tracker for fid in self.trackedFaces.keys(): t_x, t_y, t_w, t_h = self.trackedFaces[fid].getPosition() #calculate the centerpoint t_x_center = t_x + 0.5 * t_w t_y_center = t_y + 0.5 * t_h # check the centerpoint of the face is within the tracker box # plus the centerpoint of the tracker must be within the detected face box if ((t_x <= x_center <= (t_x + t_w)) and (t_y <= y_center <= (t_y + t_h)) and (x <= t_x_center <= (x + w)) and (y <= t_y_center <= (y + h))): matchedFid = fid if matchedFid is None: timePrint("Creating new face ID " + str(self.nextFaceID)) # create and store the tracker tracker = dlib.correlation_tracker() pad = self.trackOffset tracker.start_track( img, dlib.rectangle(x - pad, y - pad, x + w + pad, y + h + pad)) self.trackedFaces[self.nextFaceID] = FaceTrackingData( self.nextFaceID, tracker) # increase nextFaceID counter self.nextFaceID += 1 # increase the framecounter self.frameCounter += 1 # draw the rectangle around the tracked faces for fid in self.trackedFaces.keys(): t_x, t_y, t_w, t_h = self.trackedFaces[fid].getPosition() pad = self.trackOffset color = self.boxColor if self.trackedFaces[fid].quality < self.trackQualityBar: color = self.lowBarBoxColor cv2.rectangle(img, (t_x + pad, t_y + pad), (t_x + t_w - pad, t_y + t_h - pad), color, 1) if self.debugMode: msg = self.trackedFaces[fid].name + ' ' + str( self.trackedFaces[fid].quality) cv2.putText(img, msg, (int(t_x), int(t_y)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) self.imageShape = img.shape return img
def router(root, path1, path2, path3): request.setPaths([root, path1, path2, path3]) return BaseRequestHandler.processRequest(request) if __name__ == '__main__': port = 8008 if len(sys.argv) > 1: port = int(sys.argv[1]) authorizationList = None # create proper RequestHandlers sysHandler = PiSysRequestHandler(name='PiServerSystem', authorizationList=authorizationList) gpioHandler = PiGpioRequestHandler(name='PiServerGpio', authorizationList=authorizationList) kasaHandler = KasaHs1xxRequestHandler(name='PiServerSmartPlugs', authorizationList=authorizationList) # add smart plug to kasaHandler kasaHandler.addSmartPlug('plug01', '192.168.1.85') kasaHandler.addSmartPlug('plug02', '192.168.1.86') try: app.run(host='0.0.0.0', port=port, debug=True, threaded=True, use_reloader=False) except KeyboardInterrupt: timePrint("PiServer stopped")
def post(self, node, dataId, args): """ post data or cmd to the smart plug . str node: one of ['system', 'cmd'] . str dataId: the data ID of the node . dict args: key-value to update data property returns a tuple (statusCode, dict) where the dict contains response or data items """ # post is only valid for node in ['system', 'cmd'] # valid post to 'cmd': reboot, reset # valid post to 'system' with dataId: ['deviceId', 'hwId', 'mac', 'alias', 'led_off', 'relay_state', 'latitude_i', 'longitude_i'] statusCode = 200 response = {} nodeLowercase = node.lower() if nodeLowercase == 'system': errorCount = 0 validDataIds = { 'deviceId': 'set_device_id', 'hwId': 'set_hw_id', 'mac': 'set_mac_addr', 'alias': 'set_dev_alias' } #validDataIds2 = {'led_off': 'set_led_off', 'relay_state': 'set_relay_state'} for key, value in args.items(): if key in validDataIds: cmd = validDataIds[key] self.smartPlug.command(('system', cmd, {key: value})) timePrint('%s %s: %s' % (self.id, cmd, str(value))) response[key] = value elif key == 'led_off': self.smartPlug.command(('system', 'set_led_off', { 'off': int(value) })) timePrint('%s set_led_off: %s' % (self.id, str(value))) response[key] = int(value) elif key == 'relay_state': self.smartPlug.command(('system', 'set_relay_state', { 'state': int(value) })) timePrint('%s set_relay_state: %s' % (self.id, str(value))) response[key] = int(value) else: errorCount = +1 if errorCount > 0: if len(response) > 0: statusCode = 206 else: statusCode = 400 response[KeyResponse] = 'NoValidDataId' elif nodeLowercase == 'cmd': delay = 1 if 'delay' in args: delay = args['delay'] if dataId == 'reset': self.smartPlug.command(('system', 'reset', { 'delay': int(delay) })) timePrint('%s reset: %s' % (self.id, str(delay))) elif dataId == 'reboot': self.smartPlug.command(('system', 'reboot', { 'delay': int(delay) })) timePrint('%s reboot: %s' % (self.id, str(delay))) else: statusCode = 400 response[KeyResponse] = 'InvalidCommand' else: statusCode = 400 response[KeyResponse] = 'InvalidNodeId' return (statusCode, response)
def _wander(self, interval): """ autonomous wander around mindlessly wander states: 0 - WanderStateInit - this is the initial state. 1 - WanderStateMove - the piCar is moving forward. 2 - WanderStateStop - the picar is stopped due to obstacle 3 - WanderStateScan - scan distance for surroundings and pick the direction with farest distance 4 - WanderStateTurn - turning to the direction with the best distance. then go to init state. 5 - WanderStateBack - move the car backward if failed to find the best distance from the scan then repeat scan """ if self._wanderState == picarConst.WanderStateInit: # start move forward self.head.lookStraight() self.drive.forward(speed=self.config.getOrAddInt('auto.forwardSpeed', 60)) self._wanderState = picarConst.WanderStateMove timePrint('Wander state %i' %self._wanderState) elif self._wanderState == picarConst.WanderStateMove: # check whether the drive stopped if abs(self.drive.motor.speed) < 5: self._wanderState = picarConst.WanderStateStop timePrint('Wander state %i' %self._wanderState) elif self._wanderState == picarConst.WanderStateStop: # for now, just move to scan self._wanderState = picarConst.WanderStateScan timePrint('Wander state %i' %self._wanderState) elif self._wanderState == picarConst.WanderStateScan: # scan distance value = [] posh = [] posv = [] starth = self.config.getOrAddInt('wander.scan.starth', -90) startv = self.head.servoV.angle # use currently vertical angle endh = self.config.getOrAddInt('wander.scan.endh', 90) endv = startv inch = self.config.getOrAddInt('wander.scan.inc', 10) incv = inch value, posh, posv = self.head.scan(starth, startv, endh, endv, inch, incv) timePrint('Scan: %s' %(str(value))) # find max max = 0 maxindex = -1 index = 0 stopDistance = self.config.getOrAddFloat('distanceScan.stopDistanceInMeter', 0.2) for val in value: if val > max and val > stopDistance: max = val maxindex = index index += 1 if maxindex > -1: # found good one and turning to that direction angle = posh[maxindex] timePrint('maxindex %i value %f posh %i' %(maxindex, max, angle)) if angle > 0: angle = -self.config.getOrAddInt('wander.turnAngle', 30) else: angle = self.config.getOrAddInt('wander.turnAngle', 30) self.drive.turnSteering(angle) self.drive.backward(self.config.getOrAddInt('wander.turnSpeed', 60)) self._wanderTimer = int(self.config.getOrAddFloat('wander.turningTime', 2) / interval) self._wanderState = picarConst.WanderStateTurn timePrint('Wander state %i' %self._wanderState) else: # cannot find good one so move back self.drive.backward(self.config.getOrAddInt('auto.backwardSpeed', 60)) self._wanderTimer = int(self.config.getOrAddFloat('wander.backwardTime', 1) / interval) self._wanderState = picarConst.WanderStateBack timePrint('Wander state %i' %self._wanderState) elif self._wanderState == picarConst.WanderStateTurn: # count down the timer then stop and go to init state self._wanderTimer -= 1 if self._wanderTimer == 0: self.drive.stop() self._wanderState = picarConst.WanderStateInit timePrint('Wander state %i' %self._wanderState) elif self._wanderState == picarConst.WanderStateBack: # count down the timer then stop self._wanderTimer -= 1 if self._wanderTimer == 0: self.drive.stop() self._wanderState = picarConst.WanderStateScan timePrint('Wander state %i' %self._wanderState) self._wanderCounter += 1 if self._wanderOldState != self._wanderState: self._wanderCounter = 0 self._wanderOldState = self._wanderState if self._wanderCounter > int(self.config.getOrAddFloat('wander.stateTimeout', 10) / interval): self.drive.stop() timePrint('Wander timeout go to scan state') self._wanderState = picarConst.WanderStateScan pass
def run(commandHandler, config): #Main loop for tcp service global ap_status, wifi_status, data # load configuration motor_speed = config.getOrAddInt('tcpserver.motorSpeed', 100) spd_ad = config.getOrAddInt('tcpserver.motorSpeedAdjustment', 1) cam_turn_increment = config.getOrAddInt('tcpserver.camTurnIncrement', 4) HOST = '' PORT = config.getOrAddInt('tcpserver.tcpPort', 10223) #Define port serial BUFSIZ = 1024 #Define buffer size ADDR = (HOST, PORT) tcpSerSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) tcpSerSock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) tcpSerSock.bind(ADDR) tcpSerSock.listen(5) #Start server,waiting for client colorLower = (24, 100, 100) #The color that openCV find colorUpper = (44, 255, 255) #USE HSV value NOT RGB ap = argparse.ArgumentParser() #OpenCV initialization ap.add_argument("-b", "--buffer", type=int, default=64, help="max buffer size") args = vars(ap.parse_args()) pts = deque(maxlen=args["buffer"]) time.sleep(0.1) # Process arguments parser = argparse.ArgumentParser() parser.add_argument('-c', '--clear', action='store_true', help='clear the display on exit') args = parser.parse_args() while True: #Connection try: s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) s.connect(("1.1.1.1", 80)) ipaddr_check = s.getsockname()[0] s.close() #print(ipaddr_check) wifi_status = 1 except: if ap_status == 0: ap_threading = threading.Thread( target=ap_thread) #Define a thread for data receiving ap_threading.setDaemon( True ) #'True' means it is a front thread,it would close when the mainloop() closes ap_threading.start() #Thread starts commandHandler.drive.setLedsRGB(picarConst.YELLOW) time.sleep(2) wifi_status = 0 if wifi_status == 1: timePrint('waiting for connection IP: ' + str(ipaddr_check)) commandHandler.drive.setLedsRGB(picarConst.RED) tcpCliSock, addr = tcpSerSock.accept( ) #Determine whether to connect commandHandler.drive.setLedsRGB(picarConst.GREEN) print('...connected from :', addr) #time.sleep(1) #tcpCliSock.send(('SET %s'%vtr_mid+' %s'%hoz_mid+' %s'%left_spd+' %s'%right_spd+' %s'%look_up_max+' %s'%look_down_max).encode()) #print('SET %s'%vtr_mid+' %s'%hoz_mid+' %s'%left_spd+' %s'%right_spd+' %s'%left+' %s'%right) break else: commandHandler.drive.setLedsRGB(picarConst.BLUE) timePrint('waiting for connection IP: ' + str(ipaddr_check)) tcpCliSock, addr = tcpSerSock.accept( ) #Determine whether to connect commandHandler.drive.setLedsRGB(picarConst.GREEN) print('...connected from :', addr) #time.sleep(1) #tcpCliSock.send(('SET %s'%vtr_mid+' %s'%hoz_mid+' %s'%left_spd+' %s'%right_spd+' %s'%look_up_max+' %s'%look_down_max).encode()) #print('SET %s'%vtr_mid+' %s'%hoz_mid+' %s'%left_spd+' %s'%right_spd+' %s'%left+' %s'%right) ap_status = 1 break #FPV initialization videoPort = config.getOrAddInt('tcpserver.videoPort', 5555) context = zmq.Context() footage_socket = context.socket(zmq.PUB) footage_socket.connect('tcp://%s:' % addr[0] + str(videoPort)) #Threads start timePrint('starting video thread port: %s client: ' % videoPort + str(addr[0])) video_threading = threading.Thread( target=tcpVideoWorker, args=(commandHandler.car, footage_socket)) #Define a thread for FPV and OpenCV video_threading.setDaemon( True ) #'True' means it is a front thread,it would close when the mainloop() closes video_threading.start() #Thread starts while True: data = '' data = tcpCliSock.recv(BUFSIZ).decode() if not data: continue elif 'exit' in data: tcpCliSock.send('shutting down PiCar'.encode()) os.system("sudo shutdown -h now\n") elif 'reboot' in data: tcpCliSock.send('rebooting PiCar'.encode()) os.system("sudo reboot\n") elif 'spdset' in data: spd_ad = float((str(data))[7:]) #Speed Adjustment config.set('tcpserver.motorSpeedAdjustment', spd_ad) tcpCliSock.send(('speed set to ' + spd_ad).encode()) elif 'scan' in data: dis_can = scan(commandHandler, starth=-60, startv=0, endh=60, endv=0, inch=2, incv=2) #Start Scanning str_list_1 = dis_can #Divide the list to make it samller to send str_index = ' ' #Separate the values by space str_send_1 = str_index.join(str_list_1) + ' ' tcpCliSock.sendall((str(str_send_1)).encode()) #Send Data tcpCliSock.send( 'finished'.encode() ) #Sending 'finished' tell the client to stop receiving the list of dis_can elif 'scan_rev' in data: dis_can = scan(commandHandler, starth=60, startv=0, endh=-60, endv=0, inch=2, incv=2) #Start Scanning str_list_1 = dis_can #Divide the list to make it samller to send str_index = ' ' #Separate the values by space str_send_1 = str_index.join(str_list_1) + ' ' tcpCliSock.sendall((str(str_send_1)).encode()) #Send Data tcpCliSock.send( 'finished'.encode() ) #Sending 'finished' tell the client to stop receiving the list of dis_can elif 'EC1set' in data: # vertical servo center new_EC1 = int((str(data))[7:]) commandHandler.head.moveVertical(new_EC1) config.set('verticalServo.centerAngle', new_EC1) tcpCliSock.send('EC1set'.encode()) elif 'EC2set' in data: # horizontal servo center new_EC2 = int((str(data))[7:]) commandHandler.head.moveHorizontal(new_EC2) config.set('horizontalServo.centerAngle', new_EC2) tcpCliSock.send('EC2set'.encode()) elif 'EM1set' in data: # Motor Speed new_EM1 = int((str(data))[7:]) config.set('tcpserver.motorSpeed', new_EM1) tcpCliSock.send('EM1set'.encode()) elif 'EM2set' in data: # Motor Speed new_EM2 = int((str(data))[7:]) config.set('tcpserver.motorSpeed', new_EM2) tcpCliSock.send('EM2set'.encode()) elif 'LUMset' in data: # todo: look_up_max new_ET1 = int((str(data))[7:]) #replace_num('look_up_max:',new_ET1) #turn.camera_turn(new_ET1) tcpCliSock.send('LUMset'.encode()) elif 'LDMset' in data: # todo: look_down_max new_ET2 = int((str(data))[7:]) #replace_num('look_down_max:',new_ET2) #turn.camera_turn(new_ET2) tcpCliSock.send('LDMset'.encode()) elif 'stop' in data: #When server receive "stop" from client,car stops moving tcpCliSock.send('9 - stopping moving'.encode()) commandHandler.drive.stop() continue elif 'lightsON' in data: #Turn on the LEDs commandHandler.drive.setLedsRGB(picarConst.RGBON) tcpCliSock.send('lightsON'.encode()) elif 'ledRainbowCycle' in data: #Turn on rainbowCycle tcpCliSock.send('RainbowCycle'.encode()) led_lightshow = 1 elif 'ledTheaterChaseRainbow' in data: #Turn on theaterChaseRainbow tcpCliSock.send('TheaterChaseRainbow'.encode()) led_lightshow = 2 elif 'ledColor' in data: #Turn on leds with color - format: ledColor: rrr, ggg, bbb red = int((str(data))[10:13]) green = int((str(data))[15:18]) blue = int((str(data))[20:]) colorWipe(strip, Color(red, green, blue)) tcpCliSock.send( ('ledColor: ' + str(red) + str(green) + str(blue)).encode()) led_lightshow = 0 elif 'lightsOFF' in data: #Turn off the LEDs commandHandler.drive.setLedsRGB(picarConst.RGBOFF) tcpCliSock.send('lightsOFF'.encode()) elif 'middle' in data: #Go straight commandHandler.drive.turnStraight() tcpCliSock.send('Going straight'.encode()) elif 'Left' in data: #Turn left commandHandler.drive.turnLeft(45) tcpCliSock.send('3 - turning left'.encode()) elif 'Right' in data: #Turn right commandHandler.drive.turnRight(45) tcpCliSock.send('4 - turning right'.encode()) elif 'backward' in data: #When server receive "backward" from client,car moves backward tcpCliSock.send('2 - going backward'.encode()) commandHandler.drive.backward(motor_speed * spd_ad) elif 'forward' in data: #When server receive "forward" from client,car moves forward tcpCliSock.send('1 - going forward'.encode()) commandHandler.drive.forward(motor_speed * spd_ad) elif 'l_up' in data: #Camera look up angle = commandHandler.head.servoV.angle if angle < commandHandler.head.maxUpAngle: angle += cam_turn_increment commandHandler.head.moveVertical(angle) tcpCliSock.send('5 - move camera up'.encode()) elif 'l_do' in data: #Camera look down angle = commandHandler.head.servoV.angle if angle > commandHandler.head.maxDownAngle: angle -= cam_turn_increment commandHandler.head.moveVertical(angle) tcpCliSock.send('6 - move camera down'.encode()) elif 'l_le' in data: #Camera look left angle = commandHandler.head.servoH.angle if angle > commandHandler.head.maxLeftAngle: angle -= cam_turn_increment commandHandler.head.moveHorizontal(angle) msg = '7 - move camera left: ' + str(angle) timePrint(msg) tcpCliSock.send(msg.encode()) elif 'l_ri' in data: #Camera look right angle = commandHandler.head.servoH.angle if angle < commandHandler.head.maxRightAngle: angle += cam_turn_increment commandHandler.head.moveHorizontal(angle) msg = '8 - move camera right: ' + str(angle) timePrint(msg) tcpCliSock.send(msg.encode()) elif 'ahead' in data: #Camera look ahead commandHandler.head.lookStraight() tcpCliSock.send('recenter camera'.encode()) elif 'Stop' in data: #When server receive "Stop" from client,Auto Mode switches off tcpCliSock.send('auto_status_off'.encode()) commandHandler.car.setOperationMode(picarConst.ManualMode) elif 'auto' in data: #When server receive "auto" from client,start Auto Mode if commandHandler.car.setOperationMode( picarConst.FollowDistanceMode): tcpCliSock.send('0 - starting auto mode'.encode()) continue elif 'wander' in data: #When server receive "wander" from client,start wander Mode if commandHandler.car.setOperationMode(picarConst.AutoWanderMode): tcpCliSock.send('wandering'.encode()) continue elif 'findline' in data: #Find line mode start if commandHandler.car.setOperationMode(picarConst.FollowLineMode): tcpCliSock.send('findline'.encode()) continue elif 'voice_3' in data: #Speech recognition mode start #if set_mode_speech(): # tcpCliSock.send('voice_3'.encode()) continue