def togglePause(self):
'''
Description:
Toggles between pause state.
'''
while True:
try:
val = get('pause')
if val is dict and val != {}:
self.__pause = val["pause"]
except:
print("DS pause error")
def setQuit(self):
'''
Description:
Used to quit further autonomous traversal.
'''
while True:
try:
val = get('quit')
if val is dict and val != {}:
self.__quit = val["quit"]
except:
print("DS quit error")
def getDataFromDeepstream():
global keyIn, screen, iftop, reach, imu, mode
while True:
try:
iftop = get("speed")
except:
iftop = "NO_RECORD"
sleep(.08)
try:
reach = get("reach")
except:
reach = "NO_RECORD"
sleep(.08)
try:
imu = get("imu")
except:
imu = {}
sleep(.08)
try:
mode = get("mode")
except:
mode = {}
sleep(.08)
try:
timeDiff = get("timeDiff")
except:
timeDiff = {}
sleep(.08)
if keyIn == ord(chr(27)):
quit()
def modeChecker():
global mobilityMode
while True:
try:
m = get("mode", '192.168.1.2')
if type(m) == dict:
if "mode" in m and "roverType" in m and m != {}: #check - isn't m != {} not needed
mobilityMode = m
except:
try:
sleep(0.1)
m = get("mode", "127.0.0.1")
print("Mode: " + str(m))
if type(m) == dict:
if "mode" in m and "roverType" in m and m != {}:
mobilityMode = m
sleep(1) #check - why is sleep so long
if "roverType" in m and "roverType" in os.environ: #check - duplicate check of 1st variable
if m["roverType"] == "base" and os.environ['roverType'] == 'base':
try:
post(mobilityMode, "mode", "192.168.1.2")
except:
print("could not post the mode over to the rover")
pass
except:
print("Not getting mobility mode from local deepstream")
pass
sleep(1)
def getDataFromDeepstream():
global mode, gpsManual, shouldGoToPoints, currentLocation
while True:
try:
m = get('mode', 'localhost')
if type(m) == dict and "mode" in m:
mode = m
#print(mode)
except:
pass
sleep(0.05)
try:
p = get('gpsManual', 'localhost')
if type(p) == dict:
if 'points' in p:
if len(p['points']) > 0:
if not shouldGoToPoints:
gpsManual = p['points']
except:
pass
sleep(0.05)
try:
l = get('reach', 'localhost')
if type(l) == dict:
if 'lat' in l and 'lon' in l:
currentLocation = l
except:
pass
sleep(0.05)
def getFromDeepstream():
global points, points1
while True:
reach = get('gps')
mode = get('mode')
points1 = get('gpsManual')
if points1 != {} and len(points1['points']) != len(points):
points = points1['points']
if type(reach) == dict:
storeToDeepstream(reach)
sleep(0.1)
def setGps(self):
'''
Description:
Retrieves current GPS location from deepstream, sets self.__gps
Args:
None
Returns:
Nothing
'''
try:
self.__gps = (get("gps")["lat"], get("gps")["lon"])
except:
print("GPS error")
def checkDsButton():
global dsButton, roverActions
if (not roverActions["auto"]["held"]
and roverActions["auto"]["value"]): # New button press
roverActions["auto"]["held"] = True
roverActions["auto"]["lastpress"] = datetime.now()
if (roverActions["auto"]["held"] and not roverActions["auto"]["value"]
): # Button held, but now released
roverActions["auto"]["held"] = False
dsButton = False
if (roverActions["auto"]["held"] and roverActions["auto"]["value"]
and (datetime.now() - roverActions["auto"]["lastpress"]).seconds >=
actionTime): # Button held for required time
roverActions["auto"]["lastpress"] = datetime.now(
) # Keep updating time as button may continue to be held
dsButton = True
global dsMode
while True:
try:
post({"mobilityTime": int(time.time())}, "mobilityTime")
time.sleep(.1)
m = get("mode")
if type(m) == dict:
dsMode = m["mode"]
if prevDsMode != dsMode:
client_socket.sendto(bytes("0,0,0,0,0,0,0,0,0,1", "utf-8"),
address)
except:
print("Cannot send to Deepstream")
time.sleep(.1)
def toggleDrvPause():
global myDriver, toggleSuspend
while True:
try:
pauseRecord = get("pause")
except:
print("toggleDrvPause: DS get failed")
continue
if not pauseRecord == toggleSuspend:
myDriver.setPause()
toggleSuspend = not toggleSuspend
def stopDrv():
global myDriver, requestStop
while True:
try:
stopRecord = get("stop")
except:
print("stopDrv: DS get failed")
continue
if stopRecord == True:
myDriver.setStop()
requestStop = False
def collectPoints():
global storedPoints, ghzConnection, currentGpsLoc
prevPoint = (0.00, 0.00)
while True:
if ghzConnection:
gps = get('gps')
currentGpsLoc = (gps['lat'], gps['lon'])
if dist(prevPoint, currentGpsLoc) > 500:
storedPoints.append(currentGpsLoc)
prevPoint = currentGpsLoc
sleep(5)
def checkStop(self):
'''
Description:
Used to exit the current goTo function indefinitely.
'''
try:
stopped = get("driver")["stop"]
if stopped:
self.__stop = True
except:
print("Stop error")
def getDeepStreamCurrent():
global GNSS
while True:
for i in range(100):
post({"lat": i, "lon": i + 250}, "GNSS")
GPS = get("GNSS")
#GNSS['lat'],GNSS['lon'] = GPS["data"]["lat"], GPS["data"]["lon"]
sleep(.04)
print(GPS['lat'])
i += 10
def setStop(self):
'''
Description:
Used to exit the current goTo function indefinitely.
'''
while True:
try:
val = get('stop')
if val is dict and val != {}:
self.__stop = val["stop"]
except:
print("DS stop error")
def setHeading(self):
'''
Description:
Retrieves current heading from deepstream, sets self.__heading
Args:
None
Returns:
Nothing
'''
try:
self.__heading = get("imu")["heading"]
except:
print("Heading error")
def checkPause(self):
'''
Description:
Suspends Rover movement until further update in Deepstream.
'''
try:
pause = get("driver")["paused"]
if pause:
self.__paused = True
else:
self.__paused = False
except:
print("Pause error")
def getDeepStreamCurrent():
global INPUT_MODE
global GNSS
global DD_lat
global DD_lon
while True:
GPS = get("GNSS") #Grab from Deepstream
if (INPUT_MODE == "DMS"):
DD_lat = DMS_to_DD(GPS['lat'])
DD_lon = DMS_to_DD(GPS['lon'])
elif (INPUT_MODE == "DD"):
DD_lat = GPS['lat']
DD_lon = GPS['lon']
DD_lat = float(DD_lat)
DD_lon = float(DD_lon)
else:
print("GPSstats.py failed to run! INPUT_MODE needs to be valid.")
exit()
report_current_position()
sleep(.5)
generate_destination(180, 786.0792)
from deepstream import get, post
from time import sleep
while True:
try:
iftop = get("speed")
sleep(.1)
mode = get("mode")
print(iftop)
print(mode)
sleep(1)
except:
print("problem connecting to deepstream")
from deepstream import get
from time import sleep
ip = "10.42.0.1"
while True:
try:
data = get('imu')
print(data)
sleep(.05)
except:
print("problem connecting to deepstream")
lat1 = 0
lon1 = 0
lat2 = 0
lon2 = 0
currLat = 0
currLon = 0
heading = 0
timeDiff = 0
thread = True
global mode, mobilityTime, previousMobilityTime
mode = "manual" #The Initial mode is always manual
mobilityTime = None
try:
previousMobilityTime = get('mobilityTime')['mobilityTime']
except:
previousMobilityTime = None
'''
while True:
try:
if mobilityTime == {} or type(mobilityTime) == str:
mobilityTime = get("mobilityTime")
if type(mobilityTime) == dict:
previousMobilityTime = mobilityTime["mobilityTime"]
mobilityTime = mobilityTime["mobilityTime"]
break
except:
print("Waiting for mobility time...")
sleep(.1)
'''
def getDataFromDeepstream():
global reach, imu, mobilityTime, heading, thread, timeDiff
print("getting in getDataFromDeepstream")
sleep(0.3)
while mobilityTime == None or mobilityTime == "NO_RECORD" or mobilityTime == previousMobilityTime:
try:
mobilityTime = get(
'mobilityTime'
)["mobilityTime"] # Initializing the mobilityTime from deepstream
except:
print("Still Getting the initial Mobility Time")
sleep(0.1)
print("Initial Mobility Time : ", mobilityTime)
sleep(0.1)
t2.start()
while thread:
try:
try:
print("getting GPS")
reach = get(
"gps") # getting the entire GPS data object(json)
if type(reach) == dict:
print("passing to function storedataindeepstream")
if reach != {}:
storeDataInList(reach)
#lat = reach['lat']
#lon = reach['lon']
except:
reach = "NO_RECORD"
#print("reach : ", reach)
#print("Latitude : " + str(lat) + " Longitude : " + str(lon))
sleep(.1)
try:
print("Getting IMU")
imu = get("imu")
heading = imu[
'heading'] # getting the current heading (with respect to true North) from deepstream
#print(heading)
except:
imu = {}
print("imu : ", imu)
sleep(.1)
try:
print("Getting Mobility time stamp")
mobilityTime = get(
'mobilityTime'
)["mobilityTime"] # periodically getting the mobilityTime to compare and switch modes
print("Current Mobility Time : ", mobilityTime)
except:
print("Mobility Time : ", mobilityTime)
pass
sleep(0.1)
#print("Latitude : " + str(lat) + " Longitude : " + str(lon) + " heading : " + str(heading) + " MobilityTime : " + mobilityTime)
sleep(.1)
except KeyboardInterrupt:
print("KeyboardInterrupt")
saveKML()
thread = False
sleep(3)
try:
post({"mode": "manual"}, mode)
except:
print("There was a problem setting mode back to manual")
from time import sleep
from deepstream import get
from rviewer import *
import time
startTime = int(round(time.time()))
myViewer = Viewer()
def getTFFromString(str):
return True if str == "True" else False
try:
first = get("arrival")
lastWaypoint = first["Waypoint"]
except:
lastWaypoint = {}
arrival = {}
while True:
sleep(0.3)
data = {}
try:
data = get("roverViz") # get data payload
arrival = get("arrival")
except:
print("rViz could not get deepstream data")
if arrival != {}:
if arrival["arrivalTime"] != "start":
'''
from deepstream import get, post
from time import sleep
from serial import Serial
#from threading import Thread
# Connect to COM port, need to change the first parameter to match USB com port the poke board is attached to
try:
ser = Serial(
'/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0',
9600)
except:
print("cannot connect to serial device.")
# Loop while script is running, waiting for deepstream update sent by basestation
while True:
# Get deepstream record named poke
poke = get('poke')
if 'poke' in poke:
# If poke is true, write poke bytes to serial
if poke['poke'] == True:
# This sequence matches poke_receive.py, acts as an enclosed buffer to prevent misfires
print('Writing poke sequence, finger extended')
ser.write('\n')
ser.write('p')
ser.write('o')
ser.write('k')
ser.write('e')
# Post to deepstream that the record is false to stop repeat firing of finger
post({'poke': False}, 'poke')
roll = " NOREC"
mcal = " NOREC"
dl = " NOREC"
ul = " NOREC"
ip = " NOREC"
toSend = "Loadng"*6 + " "*6 + "Loadng"
while True:
try:
ser = serial.Serial(device, baud, timeout=.5)
except:
print("cannot connec to serial device: "+ device)
try:
d = get("imu")
heading = "{:3.1f}".format(float(d["heading"])).rjust(6)
pitch = "{:3.1f}".format(float(d["pitch"])).rjust(6)
roll = "{:3.1f}".format(float(d["roll"])).rjust(6)
mcal = ("yes" if int(d["mag"]) is 3 else "no").rjust(6)
except:
heading = " NOREC"
pitch = " NOREC"
roll = " NOREC"
mcal = " NOREC"
try:
iftop = get("speed")
if iftop is not {} or iftop is not "NO_RECORD":
ip = str(iftop["ip"]).rjust(12)[-12:]
