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