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 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 CalcPath(self, start, goal):
astar = AStar(self.nodes)
print('Calculating Shortest Path')
coords = astar.GetPath(start, goal)
if coords == None:
post({'astar': False}, 'astar')
coords = {}
#print(coords)
post({'coords': coords}, 'coords')
def sendToBaseStation():
global currentPoints
global previousPoints
while True:
try:
post({'cp': currentPoints}, "currentPoints")
post({'pp': previousPoints}, "previousPoints")
print("Successful Post")
except:
print("Cannot Send to HomeBase")
sleep(5)
def goToWhenConnectionLost():
global hasSomeMovement, shouldGoToPoints, gpsManual
while True:
while len(gpsManual) > 0:
if shouldGoToPoints and not hasMovement:
Driver.goTo(gpsManual[-1])
gpsManual.pop()
try:
post({ 'points': gpsManual }, 'gpsManual', 'localhost')
except:
pass
sleep(0.04)
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 getUpDownData():
global obj, ipAddress, upload, download, elapsedTime
while True:
try:
# command: sudo iftop -o 10s -t -s 10 -L 1 -i wlp3s0
elapsedTime = 0
p = Popen([
"/usr/sbin/iftop", "-o", "10s", "-t", "-s", "10", "-L", "1",
"-i", interface
],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE).communicate()
out = p[0]
err = p[1]
uploadArr = re.findall(
r"Total send rate:\s+(\d{1,}\.{0,1}\d{0,})(\w+)", out)
downloadArr = re.findall(
r"Total receive rate:\s+(\d{1,}\.{0,1}\d{0,})(\w+)", out)
ipAddressArr = re.findall(
r"IP address is:\s+(\d{1,3}\.\d{1,3}\.\d{1,3}\.\d{1,3})", err)
if ipAddressArr is not []:
ipAddress = ipAddressArr[0]
if uploadArr is not [] and downloadArr is not []:
upload = float(uploadArr[0][0])
download = float(downloadArr[0][0])
if uploadArr[0][1] == "Kb":
upload = upload * 1000
if downloadArr[0][1] == "Kb":
download = download * 1000
if uploadArr[0][1] == "Mb":
upload = upload * 1000000
if downloadArr[0][1] == "Mb":
download = download * 1000000
obj = {
"upload": upload,
"download": download,
"ip": ipAddress,
"elapsed": elapsedTime
}
#print "upload: {} {} download: {} {} ip: {}".format(upload, uploadArr[0][1], download , downloadArr[0][1], ipAddress)
dsSuccess = post(obj, recordName)
print(obj)
uploadArr = []
downloadArr = []
except:
try:
post({}, "speed")
print("No data from interface: " + interface)
sleep(1)
except:
print("cannot connect to deepstream.")
def checkElapsedTime():
global elapsedTime
while True:
elapsedTime = elapsedTime + 1
if(elapsedTime > 16):
try:
post({ "mode": "autonomanual" }, "mode")
upload = 0
download = 0
post({"upload": upload, "download": download, "ip": ipAddress, "elapsed": elapsedTime}, recordName)
except:
print("cannot post to deepstream")
sleep(1)
def storeToDeepstream(reach):
global Points, mode, lat1, lon1
lat2, lon2 = reach['lat'], reach['lon']
if distance((lat1, lon1),
(lat2, lon2)) > 5.0 and reach['sde'] < 10 and reach[
'sdn'] < 10 and reach['fix'] and mode == "manual":
if len(points) < 30:
points.append((lat2, lon2))
else:
del points[0]
points.append((lat2, lon2))
post({"points": points}, "gpsManual", 'localhost')
lat1 = lat2
lon1 = lon2
def requestControl():
try:
modeRecord = post({"mode": "manual"}, "mode")
print("Updated mode record:", str(modeRecord))
time.sleep(.1)
initArduinoConnection()
print("Trying to initialize a connection to the arduino...")
except:
print("Cannot access mode record")
def roverViewer(self):
'''
Description:
Updates deepstream record 'roverViz' with motor1, motor2, currentHeading,
targetDistance, targetHeading, headingDiff, and clockwise values for the rover viewer.
Args:
None
Returns:
Nothing
'''
post(
{
"motor1": self.__motor1,
"motor2": self.__motor2,
"currentHeading": self.__heading,
"targetDistance": self.__distance,
"deltaDirection": self.__deltaDirection,
"shouldCW": self.__clockwise
}, "roverViz")
time.sleep(0.04)
def notifyArrival(self):
'''
Description:
Updates deepstream record 'arrival' with self.__nextWaypoint and arrivalTime once
the destination has been reached. Flashes LED pattern for on board visual cue.
Args:
None
Returns:
Nothing
'''
self.__motor1 = self.__motor2 = 0
# Update deepstream
arrivalTime = str(time.strftime("%I:%M:%S", time.localtime()))
try:
post(
{
"Waypoint": str(self.__nextWaypoint),
"arrivalTime": arrivalTime
}, "arrival")
except:
print("Notify error")
# Blink lights
leds = [0, 1, 2]
color = toggle = 0
for i in range(20): # Blink red, green, and blue for 5 seconds
try:
t = round(time.time(), 3)
o = pack('s 10h d s', 'c'.encode('utf-8'), 0, 0, 0, 0, 0, 0, 0,
0, 0, 4, t, '#'.encode('utf-8'))
self.__client_socket.sendto(o, self.__address)
time.sleep(0.25)
toggle += 1
if toggle >= len(leds):
toggle = 0
color = leds[toggle]
except:
print("Arduino send failed")
self.sendMotors()
def switchToAutonomanual():
global mobilityTime, mode, thread, timeDiff
print("getting in switchToAutonomaual")
while thread:
sleep(.2)
#print("mobility time : ", mobilityTime)
if (type(mobilityTime) == int):
timeDiff = (mobilityTime - time.time())
post({"timeDiff": ":.2f".format(timeDiff)}, "timeDiff")
print("\nThe Time Difference is : ", timeDiff)
if mobilityTime + 10 < int(time.time()) or mode == "autonomanual":
print("checking for autonomanual mode")
if mode != "autonomanual":
try:
print(
"changing mode to autonomanual and posting to deepstream"
)
#ser = serial.Serial(device, baud, timeout = 0.5)
#ser.write("1")
#sleep(8)
mode = "autonomanual"
except:
print(
"Error In changing modes to Autonomanual so setting mode to MANUAL"
)
mode = "manual"
print("The Current Mode is : ", mode)
post({"mode": mode}, "mode")
sleep(0.3)
print("SENDING DATA TO ARDUINO TO STOP")
print("0,0,0,0,0,0,0,0,0,1")
client_socket.sendto(bytes("0,0,0,0,0,0,0,0,0,1", "utf-8"),
address)
elif int(mobilityTime) + 10 > int(time.time()):
print("Entering Into MANUAL MODE Again")
mode = "manual"
post({"mode": mode}, "mode")
print("The Current Mode is : ", mode)
sleep(0.3)
else:
try:
re_data = client_socket.recvfrom(512)
if bytes.decode(re_data[0]) == "r":
#print("SENDING DATA TO ARDUINO TO TAKE REVERSE")
#print("-20,0,0,0,0,0,0,0,0,4")
client_socket.sendto(
bytes("-20,0,0,0,0,0,0,0,0,4", "utf-8"),
address)
#returnToStart()
sleep(.05)
except:
print("Send failed")
import time
from deepstream import get, post
import BNO055
import RPi.GPIO as GPIO
magPin = 16
dsPin = 21
response = None
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(magPin, GPIO.OUT)
GPIO.setup(dsPin, GPIO.OUT)
try:
obj = {}
post(obj, 'imu')
except:
print("Not connected to deepstream")
magneticDeclination = 11.88
# Create and configure the BNO sensor connection. Make sure only ONE of the
# below 'bno = ...' lines is uncommented:
# Raspberry Pi configuration with serial UART and RST connected to GPIO 18:
bno = BNO055.BNO055(serial_port='/dev/ttyAMA0', rst=18)
# BeagleBone Black configuration with default I2C connection (SCL=P9_19, SDA=P9_20),
# and RST connected to pin P9_12:
#bno = BNO055.BNO055(busnum=2)
# Enable verbose debug logging if -v is passed as a parameter.
if len(sys.argv) == 2 and sys.argv[1].lower() == '-v':
#!/usr/bin/python
from time import sleep
from subprocess import call, Popen
from deepstream import post
import subprocess, re
obj = {}
post(obj, 'imuUpDown')
while True:
p = Popen(
["iftop", "-o", "10s", "-t", "-s", "10", "-L", "2", "-i", "usb0"],
stdout=subprocess.PIPE)
out, err = p.communicate()
uploadArr = re.findall(r"Total send rate:\s+(\d{1,}\.{0,1}\d{0,})(\w+)",
out)
downloadArr = re.findall(
r"Total receive rate:\s+(\d{1,}\.{0,1}\d{0,})(\w+)", out)
upload = 0
download = 0
if uploadArr is not [] and downloadArr is not []:
upload = float(uploadArr[0][0])
download = float(downloadArr[0][0])
if uploadArr[0][1] == "Kb":
upload = upload * 1000
if downloadArr[0][1] == "Kb":
download = download * 1000
if uploadArr[0][1] == "Mb":
upload = upload * 1000000
if downloadArr[0][1] == "Mb":
download = download * 1000000
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 deepstream import post
import subprocess, re
print("Loading: iftop")
recordName = "speed"
interface = "eth0"
# Number in seconds to switch to autonomous/manual Mode
elapsedTimout = 16
obj = {}
success = ""
while success == "":
try:
success = post(obj, recordName)
#success = post({"mode": "manual"}, "mode")
except:
print("Not connected to deepstream")
sleep(1)
ipAddress = " NOREC"
upload = 0
download = 0
elapsedTime = 0
def getUpDownData():
global obj, ipAddress, upload, download, elapsedTime
while True:
try:
while (proceed):
response = input("Do you want to fetch values (y/n) -")
if (response == 'y'):
listLength = 0
values = []
while (listLength != NoOfValues):
values = getArduinoValues()
listLength = len(values)
#print(listLength) #Prints the length of the sensor values list
DictSensorValues = ListToDict(values)
print(DictSensorValues)
saveValues = input("Do you want to save sensor values (y/n) -")
if (saveValues == "y"):
try:
mesg = deepstream.post(DictSensorValues,
"Jit_ScienceSensorData")
if (mesg.upper() == "SUCCESS"):
print(
"Sensor values posted to Deepstream record Jit_ScienceSensorData"
)
except:
print("An error occured while posting to Deepstream")
else:
proceed = False
# Arduino address and connection info
address = ("192.168.1.177", 5000)
client_socket = socket(AF_INET, SOCK_DGRAM)
client_socket.settimeout(0.5)
GNSS = {'lat': 0, 'lon': 0}
DD_lat = 0
DD_lon = 0
EarthRadius = 6378.1 #In KM
INPUT_MODE = "DD" #[DD] for decimal degrees or [DMS] for degree minutes seconds
generate_destination_times = []
if (INPUT_MODE == "DMS"):
post({
"lat": "33° 52' 54.7\" N",
"lon": "117° 53' 04.2\" W"
}, "GNSS") #DMS Format
if (INPUT_MODE == "DD"):
post({"lat": "33.881860", "lon": "-117.884492"}, "GNSS") #DD Format
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"):
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')
from deepstream import post
import subprocess, re
print("Loading: iftop")
recordName = "speed"
interface = "wlp3s0"
# Number in seconds to switch to autonomous/manual Mode
elapsedTimout = 16
obj = {}
success = ""
while success == "":
try:
success = post(obj, recordName)
success = post({"mode": "manual"}, "mode")
except:
print("Not connected to deepstream")
sleep(1)
ipAddress = " NOREC"
upload = 0
download = 0
elapsedTime = 0
def getUpDownData():
global obj, ipAddress, upload, download, elapsedTime
while True:
try:
global controls # Holds file configurations
global modeNum # Current mode index to toggle modeNames lst
global mode # Current set name (string) in use
global modeNames # List of set names (strings) from .txt file
global actionTime # Seconds needed to trigger pause / mode change
global pausedLEDs # LED settings for paused mode
paused = False
modeNum = 0
actionTime = 3
pausedLEDs = {"R": True, "G": False, "B": False} # Red for paused
print("Going through startup process...")
while True:
success = None
try:
success = post({"mode": dsMode}, "mode")
except:
pass
time.sleep(.1)
print(str(success))
if success == "SUCCESS":
break
print("Posted mode to manual...")
actionList = [
"motor1", "motor2", "arm2", "arm3", "joint1", "joint4", "joint5a",
"joint5b", "reserved1", "ledMode"
] # List in order of socket output values
#!/usr/bin/python
from time import sleep
import RPi.GPIO as GPIO
from subprocess import call, Popen
from deepstream import post
import subprocess, re
GPIO.setmode(GPIO.BCM)
buttonPin = 5
GPIO.setup(buttonPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
while True:
pressed = not GPIO.input(buttonPin)
if pressed:
p = Popen(["screen", "-ls"], stdout=subprocess.PIPE)
out, err = p.communicate()
rmatch = re.findall(r"\d{1,}\.\w+", out)
for item in rmatch:
call(["screen", "-S", item, "-X", "kill"])
sleep(3)
try:
post({}, 'imu')
except:
print("Deepstream Not available")
call(["shutdown", "now"])
sleep(.1)
def __init__(self):
self.nodes = {}
post({'astar': True}, 'astar')
