def start():
# 启动GPS
GPS_thread = GPS()
GPS_thread.start()
# 启动提示预警
remind_thread = remind.remind_thread()
remind_thread.start()
class Monitor():
def __init__(self):
#super(Monitor,self).__init__()
ecuWorkerPipe = PipeCont() # ECU <-> Worker
ecuDataPipe = PipeCont() # ECU <-> Collector
ecuControlPipe = PipeCont() # ECU <-> Application
workerDataPipe = PipeCont() # Worker <-> Collector
workerControlPipe = PipeCont() # Worker <-> Application
collectorControlPipe = PipeCont() # Collector <-> Application
loggerControlPipe = PipeCont() # Logger <-> Application
loggerDataPipe = PipeCont() # Logger <-> Collector
loggerWorkerPipe = PipeCont() # Logger <-> Worker
gpsControlPipe = PipeCont() # GPS <-> Application
self.__events = {}
workQue = Queue()
resultQue = Queue()
self.__pipes = {}
self.__pipes['ECU'] = PipeWatcher(self, ecuControlPipe.s, 'ECU->APP')
self.__pipes['WORKER'] = PipeWatcher(self, workerControlPipe.s,
'WORKER->APP')
self.__pipes['COLLECTOR'] = PipeWatcher(self, collectorControlPipe.s,
'COLLECTOR->APP')
self.__pipes['LOG'] = PipeWatcher(self, loggerControlPipe.s,
'LOG->APP')
self.__pipes['GPS'] = PipeWatcher(self, gpsControlPipe.s, 'GPS->APP')
self.__event = {}
self.__event['ISCONNECTED'] = Event()
self.__event['GETQUEUES'] = Event()
self.__event['GETCOMMANDS'] = Event()
self.__event['SUPPORTED_COMMANDS'] = Event()
self.__event['GETSTATUS'] = Event()
self.__event['SUM'] = Event()
self.__event['AVG'] = Event()
self.__event['MIN'] = Event()
self.__event['MAX'] = Event()
self.__event['VAL'] = Event()
self.__event['DATALINE'] = Event()
self.__event['LOGNAME'] = Event()
self.__event['SNAPSHOT'] = Event()
self.__event['SUMMARY'] = Event()
self.__ecu = ECU(
workQue,
ecuWorkerPipe.s, # ECU <-> Worker
ecuControlPipe.r, # ECU <-> Application
ecuDataPipe.s) # ECU <-> Collector
self.__worker = Worker(
workQue,
resultQue,
ecuWorkerPipe.r, # Worker <-> ECU
workerControlPipe.r, # Worker <-> Application
workerDataPipe.s, # Worker <-> Collector
loggerWorkerPipe.s) # Worker <-> Logger
self.__collector = Collector(
ecuDataPipe.r, # Collector <-> ECU
collectorControlPipe.r, # Collector <-> Application
loggerDataPipe.r, # Collector <-> Logger
workerDataPipe.r, # Collector <-> Worker
resultQue)
self.__logger = DataLogger(
loggerControlPipe.r, # Logger <-> Application
loggerDataPipe.s, # Logger <-> Collector
loggerWorkerPipe.r) # Logger <-> Worker
self.__gps = GPS(resultQue, gpsControlPipe.r) # GPS <-> Application
self.gpsEnable = config.getboolean('Application', 'GPS Enabled')
self.__ecu.start()
self.__collector.start()
self.__worker.start()
self.__logger.start()
self.__gps.start()
for p in self.__pipes:
self.__pipes[p].start()
# monitor needs to listen for a connection event triggered by WORKER
def connection(self):
if self.isConnected:
self.resume()
else:
self.pause()
def addQue(self, que, frequency):
self.__pipes['ECU'].send(
Message('ADDQUE', QUE=que, FREQUENCY=frequency))
def addCommand(self, que, command, override=False):
self.__pipes['ECU'].send(
Message('ADDCOMMAND', QUE=que, COMMAND=command, OVERRIDE=override))
def setQueFrequency(self, que, frequency):
self.__pipes['ECU'].send(
Message('SETFREQUENCY', QUE=que, FREQUENCY=frequency))
def deleteAfterPoll(self, que, flag):
self.__pipes['ECU'].send(Message('DELETEAFTERPOLL', QUE=que,
FLAG=flag))
def stop(self):
self.__pipes['ECU'].send(Message('STOP'))
self.__pipes['COLLECTOR'].send(Message('STOP'))
self.__pipes['WORKER'].send(Message('STOP'))
self.__pipes['LOG'].send(Message('STOP'))
self.__pipes['GPS'].send(Message('STOP'))
logging.info('Joining processes')
self.__collector.join()
logging.info('Collector joined')
self.__worker.join()
logging.info('Worker joined')
self.__logger.join()
logging.info('Logger joined')
self.__gps.join()
logging.info('GPS Joined')
self.__ecu.join()
logging.info('ECU Joined')
def pause(self):
self.__pipes['ECU'].send(Message('PAUSE'))
self.__pipes['LOG'].send(Message('PAUSE'))
self.__pipes['GPS'].send(Message('PAUSE'))
def resume(self):
logger.info('Resuming co-processes')
self.__pipes['ECU'].send(Message('RESUME'))
self.__pipes['LOG'].send(Message('RESUME'))
self.__pipes['GPS'].send(Message('RESUME'))
def reset(self):
self.__pipes['COLLECTOR'].send(Message('RESET'))
def save(self):
self.__pipes['LOG'].send(Message('SAVE'))
def discard(self):
self.__pipes['LOG'].send(Message('DISCARD'))
def logPath(self, path):
self.__pipes['LOG'].send(Message('LOGPATH', PATH=path))
def logFrequency(self, frequency):
self.__pipes['LOG'].send(Message('FREQUENCY', FREQUENCY=frequency))
def logHeadings(self, headings):
self.__pipes['LOG'].send(
Message('ADD_HEADINGS', HEADINGS=headings, INSERT=True))
@property
def isConnected(self):
self.__connected_return = None # Stores the response from the callback
self.__pipes['WORKER'].send(
Message('CONNECTED')) # Send message for incomming request
self.__event['ISCONNECTED'].wait()
self.__event['ISCONNECTED'].clear()
return self.__connected_return[
'STATUS'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def connected(self, p): # Callback function for IsConnected
self.__connected_return = p # Store the response returned for the caller to find
self.__event['ISCONNECTED'].set()
@property
def queues(self):
self.__queues_return = None # Stores the response from the callback
self.__pipes['ECU'].send(
Message('GETQUEUES')) # Send message for incomming request
self.__event['GETQUEUES'].wait()
self.__event['GETQUEUES'].clear()
return self.__queues_return[
'QUEUES'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getqueues(self, p): # Callback function for IsConnected
self.__queues_return = p # Store the response returned for the caller to find
self.__event['GETQUEUES'].set()
@property
def commands(self):
self.__commands_return = None # Stores the response from the callback
self.__pipes['WORKER'].send(
Message('GETCOMMANDS')) # Send message for incomming request
self.__event['GETCOMMANDS'].wait()
self.__event['GETCOMMANDS'].clear()
return self.__commands_return[
'COMMANDS'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getcommands(self, p): # Callback function for IsConnected
self.__commands_return = p # Store the response returned for the caller to find
self.__event['GETCOMMANDS'].set()
@property
def supportedCommands(self):
self.__s_commands_return = None # Stores the response from the callback
self.__pipes['WORKER'].send(Message(
'SUPPORTED_COMMANDS')) # Send message for incomming request
self.__event['SUPPORTED_COMMANDS'].wait()
self.__event['SUPPORTED_COMMANDS'].clear()
return self.__s_commands_return[
'SUPPORTED_COMMANDS'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def supported_commands(self, p): # Callback function for IsConnected
self.__s_commands_return = p # Store the response returned for the caller to find
self.__event['SUPPORTED_COMMANDS'].set()
@property
def status(self):
s = {}
self.__s_ecu_return = None # Stores the response from the callback
self.__s_data_return = None # Stores the response from the callback
self.__s_worker_return = None # Stores the response from the callback
self.__s_log_return = None # Stores the response from the callback
self.__s_gps_return = None # Stores the response from the callback
self.__pipes['WORKER'].send(
Message('GETSTATUS')) # Send message for incomming request
self.__event['GETSTATUS'].wait()
self.__event['GETSTATUS'].clear()
s['WORKER'] = self.__s_worker_return['STATUS']
self.__pipes['ECU'].send(
Message('GETSTATUS')) # Send message for incomming request
self.__event['GETSTATUS'].wait()
self.__event['GETSTATUS'].clear()
s['ECU'] = self.__s_ecu_return['STATUS']
self.__pipes['COLLECTOR'].send(
Message('GETSTATUS')) # Send message for incomming request
self.__event['GETSTATUS'].wait()
self.__event['GETSTATUS'].clear()
s['COLLECTOR'] = self.__s_data_return['STATUS']
self.__pipes['LOG'].send(
Message('GETSTATUS')) # Send message for incomming request
self.__event['GETSTATUS'].wait()
self.__event['GETSTATUS'].clear()
s['LOG'] = self.__s_log_return['STATUS']
self.__pipes['GPS'].send(
Message('GETSTATUS')) # Send message for incomming request
self.__event['GETSTATUS'].wait()
self.__event['GETSTATUS'].clear()
s['GPS'] = self.__s_gps_return['STATUS']
return s # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def workerstatus(self, p): # Callback function for IsConnected
self.__s_worker_return = p # Store the response returned for the caller to find
self.__event['GETSTATUS'].set()
# Callback function must be lower case of the message it is to respond to
def ecustatus(self, p): # Callback function for IsConnected
self.__s_ecu_return = p # Store the response returned for the caller to find
self.__event['GETSTATUS'].set()
# Callback function must be lower case of the message it is to respond to
def datastatus(self, p): # Callback function for IsConnected
self.__s_data_return = p # Store the response returned for the caller to find
self.__event['GETSTATUS'].set()
# Callback function must be lower case of the message it is to respond to
def logstatus(self, p): # Callback function for IsConnected
self.__s_log_return = p # Store the response returned for the caller to find
self.__event['GETSTATUS'].set()
# Callback function must be lower case of the message it is to respond to
def gpsstatus(self, p): # Callback function for IsConnected
self.__s_gps_return = p # Store the response returned for the caller to find
self.__event['GETSTATUS'].set()
def sum(self, name):
self.__sum_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(Message(
'SUM', NAME=name)) # Send message for incomming request
self.__event['SUM'].wait()
self.__event['SUM'].clear()
return self.__sum_return[
'SUM'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getsum(self, p): # Callback function for IsConnected
self.__sum_return = p # Store the response returned for the caller to find
self.__event['SUM'].set()
def avg(self, name):
self.__avg_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(Message(
'AVG', NAME=name)) # Send message for incomming request
self.__event['AVG'].wait()
self.__event['AVG'].clear()
return self.__avg_return[
'AVG'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getavg(self, p): # Callback function for IsConnected
self.__avg_return = p # Store the response returned for the caller to find
self.__event['AVG'].set()
def min(self, name):
self.__min_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(Message(
'MIN', NAME=name)) # Send message for incomming request
self.__event['MIN'].wait()
self.__event['MIN'].clear()
return self.__MIN_return[
'MIN'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getmin(self, p): # Callback function for IsConnected
self.__min_return = p # Store the response returned for the caller to find
self.__event['MIN'].set()
def max(self, name):
self.__max_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(Message(
'MAX', NAME=name)) # Send message for incomming request
self.__event['MAX'].wait()
self.__event['MAX'].clear()
return self.__max_return[
'MAX'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getmax(self, p): # Callback function for IsConnected
self.__max_return = p # Store the response returned for the caller to find
self.__event['MAX'].set()
def val(self, name):
self.__val_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(Message(
'VAL', NAME=name)) # Send message for incomming request
self.__event['VAL'].wait()
self.__event['VAL'].clear()
return self.__val_return[
'VAL'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getval(self, p): # Callback function for IsConnected
self.__val_return = p # Store the response returned for the caller to find
self.__event['VAL'].set()
def dataLine(self, name):
self.__dataline_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(Message(
'DATALINE', NAME=name)) # Send message for incomming request
self.__event['DATALINE'].wait()
self.__event['DATALINE'].clear()
return self.__dataline_return[
'LINE'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def data_line(self, p): # Callback function for IsConnected
self.__dataline_return = p # Store the response returned for the caller to find
self.__event['DATALINE'].set()
@property
def gpsEnable(self):
return self.__gpsEnabled
@gpsEnable.setter
def gpsEnable(self, v):
self.__gpsEnabled = v
if getBlockPath(config.get('Application', 'GPS Vendor ID'),
config.get('Application', 'GPS Product ID')) is None:
logger.warning('GPS Device not found. Disabling GPS.')
self.__gpsEnabled = False
if self.__gpsEnabled:
#self.__pipes['GPS'].send(Message('RESUME'))
self.__pipes['LOG'].send(
Message('ADD_HEADINGS',
HEADINGS=[
'LATITUDE', 'LONGITUDE', 'ALTITUDE', 'GPS_SPD',
'HEADING'
]))
else:
self.__pipes['GPS'].send(Message('PAUSE'))
self.__pipes['LOG'].send(
Message('REMOVE_HEADINGS',
HEADINGS=[
'LATITUDE', 'LONGITUDE', 'ALTITUDE', 'GPS_SPD',
'HEADING'
]))
@property
def snapshot(self):
self.__snapshot_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(
Message('SNAPSHOT')) # Send message for incomming request
self.__event['SNAPSHOT'].wait()
self.__event['SNAPSHOT'].clear()
return self.__snapshot_return[
'SNAPSHOT'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def snap_shot(self, p): # Callback function for IsConnected
self.__snapshot_return = p # Store the response returned for the caller to find
self.__event['SNAPSHOT'].set()
@property
def logName(self):
self.__logname_return = None # Stores the response from the callback
self.__pipes['LOG'].send(
Message('LOGNAME')) # Send message for incomming request
self.__event['LOGNAME'].wait()
self.__event['LOGNAME'].clear()
return self.__logname_return[
'NAME'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def log_name(self, p): # Callback function for IsConnected
self.__logname_return = p # Store the response returned for the caller to find
self.__event['LOGNAME'].set()
@property
def summary(self):
self.__summary_return = None # Stores the response from the callback
self.__pipes['COLLECTOR'].send(
Message('SUMMARY')) # Send message for incomming request
self.__event['SUMMARY'].wait()
self.__event['SUMMARY'].clear()
return self.__summary_return[
'SUMMARY'] # Return the response from the callback to the caller
# Callback function must be lower case of the message it is to respond to
def getsummary(self, p): # Callback function for IsConnected
self.__summary_return = p # Store the response returned for the caller to find
self.__event['SUMMARY'].set()
from gps import GPS
from remind import remind_thread
if __name__ == '__main__':
# 启动GPS
GPS_thread = GPS()
GPS_thread.start()
# 启动提示预警
remind_thread = remind_thread()
remind_thread.start()
# 启动RGB灯带
from gps import GPS
from rfcomm import RFCOMM
import time
import subprocess
print("STARTING...")
rf = RFCOMM()
gps = GPS()
subprocess.run(["hciconfig", "hci0", "piscan"])
while True:
if rf.client_connected:
data = gps.update()
if data is not None:
print("DATA: " + data)
rf.send_data(data + "\r\n")
time.sleep(0.1)
else:
gps.stop()
print("WAIT FOR CLIENT...")
rf.wait_for_client()
print("CLIENT ACCEPTED")
gps.start()
print("ENABLE GPS")
class Server:
def __init__(self):
# create the grid
self.grid_width = 2.5
self.grid_height = 3.5
self.grid_x = 20
self.grid_y = 28
self.offset_x = 0
self.offset_y = 0
self.border_thickness = 1
self.use_diagonals = True
self.grid = Grid(self.grid_width, self.grid_height, self.grid_x,
self.grid_y, self.offset_x, self.offset_y,
self.border_thickness, self.use_diagonals)
# initialize default variables
self.can_run = True
self.rover_position = self.grid.get_node(0, 0)
self.destinations = []
self.home = self.grid.get_node(1, 1)
self.current_path = []
self.simple_path = []
self.mode = 0
self.status = 0
self.position_log = []
self.position_log.append(self.home)
self.sim_type = 0
self.sim_destinations = [
] # where destinations are held during certain simulations
# initialize the gpg
self.gpg = AdvancedGoPiGo3(25, use_mutex=True) # was true
volt = self.gpg.volt()
print("Current Voltage: ", volt)
if volt < 8:
print(
"Critical! voltage is very low, please charge the batteries before continuing! You have been warned!"
)
elif volt < 9:
print(
"Warning, voltage is getting low, impaired performance is expected."
)
# initialize send queue
self.send_queue = queue.Queue()
# initialize the socket
print("Awaiting connection")
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
address = (HOST, PORT)
self.socket.bind(address)
self.socket.listen(1)
# set up states
self.change_status(IDLE)
self.change_mode(AUTO)
# get the connection
self.conn, self.addr = self.socket.accept()
print("Successful connection from ", self.addr)
# initialize gps
self.gps_queue = queue.Queue()
self.gps = GPS(10, 1, self.gpg, q=self.gps_queue, debug_mode=False)
self.gps.set_min_distance(50)
self.gps_can_run = False
self.gps.set_obstacle_callback(self.obstacle_found)
self.gps.set_position_callback(self.rover_position_change)
self.gps.set_no_obstacle_callback(self.on_no_obstacles)
self.gps.set_reached_point_callback(self.destination_reached)
self.gps.set_speed(800)
# start remote control thread
self.remote_can_run = False
# start the video server
self.video = VideoServer()
self.video.start()
self.gps.start()
# this method manages incoming and outgoing commands
def manage_commands(self):
while self.can_run:
# empty the queue and send all the data to the client
while not self.send_queue.empty():
data = self.send_queue.get_nowait()
if self.mode != SIM:
self.conn.send(str.encode(data))
# wait a moment for a response
received, _, _ = select.select([self.conn], [], [], .1)
# if not response then continue
if received:
data = self.conn.recv(1024).decode('utf-8').split()
self.parse_data(data)
if self.mode == SIM:
self.update_sim_behavior()
# this method parses the incoming data commands. Each command comprises of atleast one letter that is removed
# from the list.
def parse_data(self, data):
while len(data) > 0:
command = data.pop(0)
print(command)
# GUI INPUT COMMANDS
# node has been changed
if command == "MODE":
mode = int(data.pop(0))
self.change_mode(mode)
if self.mode == SIM:
data = []
self.start_sim_behavior()
if self.mode != SIM:
if command == 'N':
x = int(data.pop(0))
y = int(data.pop(0))
node_type = int(data.pop(0))
self.node_changed(x, y, node_type)
# destination change
elif command == 'D':
x = int(data.pop(0))
y = int(data.pop(0))
if x == -1 and y == -1:
print("cleared destinations")
self.destinations = []
self.current_path = []
self.simple_path = []
self.gps.cancel_movement()
else:
node = self.grid.nodes[x][y]
if node.node_type == OPEN_SPACE and node != self.rover_position:
self.destinations.append(node)
self.find_path()
# home change
elif command == 'H':
x = int(data.pop(0))
y = int(data.pop(0))
node = self.grid.nodes[x][y]
if node.node_type == OPEN_SPACE:
self.home = node
# go
elif command == 'GO':
self.start_navigation()
# REMOTE CONTROL COMMANDS
# STOP
elif command == 'S':
self.stop_navigation()
elif command == 'Q':
print("Quitting")
self.can_run = False
# move
elif command == 'M':
# the numbers coming in should be integers, but aren't always
x_speed = int(float(data.pop(0)))
y_speed = int(float(data.pop(0)))
# adjust to proper speeds
if x_speed == 0 and y_speed == 0:
self.gpg.stop()
elif x_speed == 0:
self.gpg.rotate_right_forever()
elif y_speed == 0:
self.gpg.rotate_left_forever()
else:
self.gpg.set_left_wheel(abs(x_speed))
self.gpg.set_right_wheel(abs(y_speed))
if y_speed > 25 and x_speed > 25:
self.gpg.backward()
else:
self.gpg.forward()
# LEDS on
elif command == "LON":
print("Turning LED on")
self.gpg.led_on(0)
self.gpg.led_on(1)
self.gpg.open_left_eye()
# LEDS off
elif command == "LOFF":
print("Turning LED off")
self.gpg.led_off(0)
self.gpg.led_off(1)
self.gpg.close_left_eye()
elif command == "SIM":
self.sim_type = int(data.pop(0))
def start_sim_behavior(self):
if self.sim_type == WAIT:
self.sim_run = self.gps_can_run
self.stop_navigation()
self.gpg.stop()
self.change_status(IDLE)
elif self.sim_type == GO_BACK:
self.stop_navigation()
self.gpg.stop()
if len(self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
if self.destinations[0] == self.rover_position and len(
self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
print("log pulled")
self.start_navigation()
elif self.sim_type == CONTINUE:
print(self.gps_can_run)
self.sim_run = self.gps_can_run
self.stop_navigation()
self.gpg.stop()
if self.sim_run:
self.start_navigation()
elif self.sim_type == GO_HOME:
print("GO home start")
self.sim_destinations = list(self.destinations)
self.destinations = []
self.destinations.append(self.home)
self.start_navigation()
def update_sim_behavior(self):
if self.sim_type == WAIT:
print("Can run: ", self.gps_can_run)
self.stop_navigation()
self.change_status(IDLE)
if self.sim_type == CONTINUE and len(self.destinations) > 0:
if self.status == IDLE:
self.start_navigation()
elif len(self.destinations) == 0:
self.change_status(IDLE)
if self.sim_type == GO_BACK and self.status == IDLE:
if len(self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
if self.destinations[0] == self.rover_position and len(
self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
self.start_navigation()
elif self.rover_position != self.home:
self.destinations.append(self.home)
self.start_navigation()
def end_sim_behavior(self):
print("ending simulation")
if self.sim_type == WAIT:
self.gps_can_run = self.sim_run
print("GPS can run again", self.gps_can_run)
if self.gps_can_run:
self.start_navigation()
if self.sim_type == GO_HOME:
self.stop_navigation()
self.destinations = list(self.sim_destinations)
def start_navigation(self):
print("starting navigation")
self.find_path()
self.gps_can_run = True
if len(
self.destinations
) > 0 and self.home is not None and self.destinations[0] == self.home:
self.change_status(TO_HOME)
else:
self.change_status(TO_POINT)
self.next_gps_point()
def stop_navigation(self):
print("stopping navigation")
self.gps.cancel_movement()
self.gpg.stop()
self.gps_can_run = False
self.change_status(IDLE)
def node_changed(self, x, y, node_type):
self.grid.set_node(x, y, node_type)
if node_type == OBSTACLE:
self.add_obstacle(self.grid.nodes[x][y])
self.find_path()
def node_changed_to_open(self, node, node_type):
self.grid.set_node_type(node, node_type)
self.send_message("N " + str(node.gridPos.x) + " " +
str(node.gridPos.y) + " 0")
self.find_path()
def on_no_obstacles(self, position):
if position.x > 0 and position.y > 0 and position.x <= self.grid_width and position.y <= self.grid_height:
node = self.grid.node_from_global_coord(position)
path, _ = self.grid.find_path(self.rover_position, node, False)
for p in path:
if p.node_type != 0:
if p.gridPos.x > 0 and p.gridPos.y > 0 and p.gridPos.x < self.grid.nodes_in_x and\
p.gridPos.y < self.grid.nodes_in_y:
self.node_changed_to_open(p, 0)
def rover_position_change(self, position):
self.send_message("RT " + str(position.x) + " " + str(position.y))
# We only care about it if it is in the grid.
if position.x > 0 and position.y > 0 and position.x <= self.grid_width and position.y <= self.grid_height:
node = self.grid.node_from_global_coord(position)
# we still only care if it moves it to a new node.
if node != self.rover_position:
print("callback-rover position changed")
self.rover_position = node
# if we arrived at a destination we are done.
## if len(self.destinations) > 0 and node == self.destinations[0]:
## self.destinations = []
## self.current_path = []
# we need a new full route.
self.send_path()
# send info along
self.send_message("R " + str(node.gridPos.x) + " " +
str(node.gridPos.y))
def destination_reached(self, pos):
print("callback-point reached", pos)
# send the next point to the gps
self.next_gps_point()
# if we are at our final destination, we are done.
if len(self.destinations
) > 0 and self.destinations[0] == self.rover_position:
print("final destination reached")
self.send_message("DR")
self.stop_navigation()
if len(self.destinations) > 0:
if self.mode != SIM:
self.position_log.append(self.destinations[0])
self.destinations.pop(0)
self.start_navigation()
else:
self.find_path()
def obstacle_found(self, position):
# We only care about it if it is in the grid.
if position.x > 0 and position.y > 0 and position.x <= self.grid_width and position.y <= self.grid_height:
node = self.grid.node_from_global_coord(position)
# We still only care if this changes anything
if node.node_type != 1:
print("callback-obstacle found")
# we have an obstacle!
self.add_obstacle(node)
path, _ = self.grid.find_path(self.rover_position, node, False)
if len(path) > 0:
path.pop(-1)
for p in path:
if p.node_type != 0:
if p.gridPos.x > 0 and p.gridPos.y > 0 and p.gridPos.x < self.grid.nodes_in_x and \
p.gridPos.y < self.grid.nodes_in_y:
self.node_changed_to_open(p, 0)
def add_obstacle(self, node):
# if we have a valid node to work with.
if node.node_type != 1 and node != self.rover_position:
# we need to spread the grid.
self.grid.set_node_type(node, 1)
border = self.grid.all_borders
self.send_message("N " + str(node.gridPos.x) + " " +
str(node.gridPos.y) + " 1")
print("N", node.gridPos.x, node.gridPos.y)
# IF there is a destination in play and it is hit by the border, it needs to be cleared.
if len(self.destinations) > 0:
if border.__contains__(
self.destinations[0]) or node == self.destinations[0]:
self.destinations.pop(0)
self.current_path = []
self.simple_path = []
# we need a new path
print("obstacle, new path")
self.find_path()
# if we are currently in motion. let's go!
if self.gps_can_run and len(self.simple_path) > 0:
destination = self.grid.get_global_coord_from_node(
self.simple_path[0])
#self.gpg.stop()
self.gps_queue.queue.clear()
self.gps.cancel_movement()
self.gps_queue.put(destination)
def find_path(self, send_message=True):
if len(self.destinations) > 0:
_, self.simple_path = self.grid.find_path(self.rover_position,
self.destinations[0])
# send paths
if send_message:
print("sending paths")
self.send_path()
self.send_simple_path()
def next_gps_point(self):
# if we actually have somewhere to go
if len(self.simple_path) > 0:
print("navigating to next point!")
node = self.simple_path[0]
if node == self.rover_position and len(self.simple_path) > 0:
node = self.simple_path.pop(0)
destination = self.grid.get_global_coord_from_node(node)
self.gps_queue.put(destination)
self.send_simple_path()
#else:
#print("no more points!")
#self.stop_navigation()
def send_message(self, message):
# puts a message in the send queue
self.send_queue.put((" " + message))
def send_status(self):
self.send_message("ST " + str(self.status))
def send_path(self):
if len(self.simple_path) > 0:
self.current_path, _ = self.grid.find_path(self.rover_position,
self.simple_path[0])
for i in range(1, len(self.simple_path)):
temp, _ = self.grid.find_path(self.simple_path[i - 1],
self.simple_path[i])
self.current_path += temp
# sends the full path to the client
if len(self.current_path) > 0:
message = "FP "
for p in self.current_path:
message += p.__str__() + " "
message += "D"
self.send_message(message)
else:
self.send_message("FP D")
def send_simple_path(self):
# sends the simplified path to the client
if len(self.simple_path) > 0:
message = "SP "
for p in self.simple_path:
message += p.__str__() + " "
message += "D"
self.send_message(message)
else:
self.send_message("SP D")
def change_status(self, status):
self.status = status
if status == IDLE:
self.gpg.set_left_eye_color(BLUE)
elif status == TO_POINT:
self.gpg.set_left_eye_color(GREEN)
else: # status == TO_HOME:
self.gpg.set_left_eye_color(YELLOW)
self.gpg.open_left_eye()
self.send_status()
def change_mode(self, mode):
# handle lights
if mode == AUTO:
self.gpg.set_right_eye_color(CYAN)
self.gpg.open_eyes()
elif mode == SIM:
print("starting sim " + str(self.sim_type))
self.gpg.set_right_eye_color(RED)
if self.sim_type == 1:
self.gpg.set_antenna_color(BLUE)
elif self.sim_type == 2:
self.gpg.set_antenna_color(ORANGE)
elif self.sim_type == 3:
self.gpg.set_antenna_color(GREEN)
else: # self.sim_type == 4:
self.gpg.set_antenna_color(YELLOW)
self.gpg.open_all_leds()
else: # mode == MAN:
self.stop_navigation()
self.gpg.set_eye_color(PURPLE)
self.gpg.open_eyes()
self.gpg.close_left_eye()
# handle state change
if self.mode == MAN:
self.gpg.open_left_eye()
elif self.mode == SIM:
self.gpg.close_antenna()
# Send all obstacles
for node in self.grid.all_obstacles:
self.send_message("N " + str(node.gridPos.x) + " " +
str(node.gridPos.y) + " 1")
# Send all destinations
temp = "DU "
for node in self.destinations:
temp += str(node.gridPos.x) + " " + str(node.gridPos.y) + " "
temp += " UD"
self.send_message(temp)
# Send paths
self.send_path()
self.send_simple_path()
# Send position
self.send_message("R " + str(self.rover_position.gridPos.x) + " " +
str(self.rover_position.gridPos.y))
# end any behaviors
self.end_sim_behavior()
self.mode = mode
