def importSelected(self):
''' Import selected projects '''
numSelected = self.countRowsSelected(self.tblProjects)
if numSelected != 0:
# confirm import
if numSelected == 1:
msg = "Are you sure you want to import the selected project?"
else:
msg = "Are you sure you want to to import the selected projects?"
ret = QMessageBox.question(self,"Confirm Import", msg, QMessageBox.Yes|QMessageBox.No)
# if import is rejected return without deleting
if ret == QMessageBox.No:
return
# get project id of the selected projects
selectedRows = self.getSelectedRows(self.tblProjects)
selectedProjects = []
for row in selectedRows:
selectedProjects.append( self.tblProjects.model().item(row,0).text() )
# import selected projects
controller = Controller()
controller.importProjects( str(self.txtFilename.text()), selectedProjects )
# import message
QMessageBox.information(self,"Importing Data from Access","The selected projects have been imported.")
self.close()
else:
QMessageBox.information(self,"Import Data from Access","Please select the rows containing projects to be imported.")
def showProject(self):
controller = Controller()
projectdata = controller.getProjectFromFile( str(self.txtFilename.text()) )
self.lblProjectName.setText( projectdata["projectname"] )
self.lblStartDate.setText( projectdata["startdate"] )
self.lblCurrency.setText( projectdata["currency"] )
def loadCharacteristics(self):
model = QStandardItemModel(1,1)
# set model headers
model.setHorizontalHeaderItem(0,QStandardItem('Characteristic'))
model.setHorizontalHeaderItem(1,QStandardItem('Data Type'))
model.setHorizontalHeaderItem(2,QStandardItem('Description'))
# get characteristics
controller = Controller()
characteristics = controller.getGlobalCharacteristics("Household")
num = 0
for char in characteristics:
qtName = QStandardItem( char.name )
qtDataType = QStandardItem( char.datatypestr )
qtDescription = QStandardItem( char.description )
model.setItem( num, 0, qtName )
model.setItem( num, 1, qtDataType)
model.setItem( num, 2, qtDescription)
num = num + 1
self.tblHouseholdCharacteristics.setModel(model)
self.tblHouseholdCharacteristics.resizeColumnsToContents()
self.tblHouseholdCharacteristics.show()
def loadProjects(self):
model = QStandardItemModel(1,1)
# set model headers
model.setHorizontalHeaderItem(0,QStandardItem('Project ID'))
model.setHorizontalHeaderItem(1,QStandardItem('Project Name'))
model.setHorizontalHeaderItem(2,QStandardItem('Date of Data Collection'))
controller = Controller()
rows = controller.getProjectsFromAccess( str(self.txtFilename.text()) )
num = 0
for row in rows:
qtID = QStandardItem( str (row.ProjectID) )
qtName = QStandardItem( row.ProjectName )
qtDateCollected = QStandardItem( str( row.DateOfDataCollection ) )
model.setItem( num, 0, qtID )
model.setItem( num, 1, qtName)
model.setItem( num, 2, qtDateCollected)
num = num + 1
self.tblProjects.setModel(model)
self.tblProjects.resizeColumnsToContents()
self.tblProjects.show()
def saveMember(self):
''' Saves changes to household to database '''
# get the data entered by user
sex = self.cboSex.currentText()
age = self.txtAge.text()
if ( sex == "Male"):
memberid = "m%s" % age
else:
memberid = "f%s" % age
education = ""
yearofbirth = self.cmbYearOfBirth.currentText()
if self.chkHeadHousehold.isChecked():
headhousehold = "Yes"
else:
headhousehold = "No"
pid = self.parent.parent.projectid
periodaway = self.cmbMonthsAbsent.currentText()
reason = self.txtReason.text()
whereto = self.txtWhere.text()
controller = Controller()
household = controller.getProject(pid).getHousehold(self.hhid)
household.addMember(memberid, yearofbirth, headhousehold, sex, education, periodaway, reason, whereto)
# close new project window
self.parent.retrieveHouseholdMembers()
self.mdiClose()
def showProject(self):
controller = Controller()
project = controller.getProject( self.pid )
self.lblProjectName.setText( project.projectname )
self.lblStartDate.setText( str(project.startdate) )
self.lblCurrency.setText( project.currency )
def _init_component(self):
"""
初始化各组件
:return:
"""
self.frame_catcher = FrameCatcher()
self.percepter = Percepter()
self.controller = Controller()
def getHouseholdData(self):
''' Retrieve and display household data '''
controller = Controller()
household = controller.getProject(self.projectid).getHousehold(self.hhid)
hhid = household.hhid
householdname = household.householdname
dateofcollection = household.dateofcollection
self.txtShortHouseHoldName.setText(str(hhid))
self.dtpDateVisted.setDate(dateofcollection)
self.txtHouseholdName.setText(householdname)
def exportProject(self):
''' export project data to selected file '''
mydialog = QtGui.QFileDialog()
filename = mydialog.getSaveFileName(self, 'Export to', '.', 'IHM file (*.ihm)')
if filename:
controller = Controller()
controller.exportIhmProject(self.pid, filename)
QMessageBox.information(self,"Exporting Project to IHM file","Project data has been exported to "+filename)
self.close()
def moveAllHouseholdCharacteristics(self):
''' Add all available Household Characteristics to Project'''
row = 0
controller = Controller()
globalchars = controller.getGlobalCharacteristics("Household")
for char in globalchars:
if self.project.existsProjectCharacteristic(char.name):
msg = "The household characteristic labelled, %s, has already been added to project" % (char.name)
QMessageBox.information(self,"Project Configuration",msg)
else:
self.project.addProjectCharacteristic(char.name, char.chartype, char.datatype)
self.displaySelectedHouseholdCharacteristics()
def removeAllHouseholdCharacteristics(self):
''' remove all listed household characteristics from Project'''
msg = "Are you sure you want to remove all selected characteristics from this project?"
ret = QMessageBox.question(self,"Confirm Deletion", msg, QMessageBox.Yes|QMessageBox.No)
# if deletion is rejected return without deleting
if ret == QMessageBox.No:
return
controller = Controller()
chars = controller.getGlobalCharacteristics("Household")
for char in chars:
self.project.delProjectCharacteristic(char.name)
self.displaySelectedHouseholdCharacteristics()
def main(user_interface):
controller = Controller()
user_interface = _get_user_interface(ui=user_interface)
with Bridgehead(tty='/dev/ttyACM0', baudrate=9600) as bridgehead:
rpms = [None] * len(Configuration.fans)
while True:
controller.control(rpms)
user_interface.update()
try:
rpms = bridgehead(pwms=State().pwms)
except KeyboardInterrupt:
break
def main(user_interface):
controller = Controller()
user_interface = _get_user_interface(ui=user_interface)
ttys = ['/dev/ttyACM0', '/dev/ttyACM1', '/dev/ttyACM2', '/dev/ttyACM3']
with Bridgehead(ttys=ttys, baudrate=9600) as bridgehead:
rpms = [None] * len(Configuration.fans)
while True:
controller.control(rpms)
user_interface.update()
try:
rpms = bridgehead(pwms=State().pwms)
except KeyboardInterrupt:
break
def __init__(self, parent):
''' Set up the dialog box interface '''
QDialog.__init__(self)
self.parent = parent
self.dietid = 0 # selected diet item
self.currentItem = "" # selected standard of living item
self.setupUi(self)
controller = Controller()
self.project = controller.getProject( self.parent.projectid )
# show first tab first
self.tabProject.setCurrentIndex(0)
# display project name
self.lblProjectName.setText(self.parent.projectname)
# display Available and Selected Household Characteristics
self.displayAvailableHouseholdCharacteristics()
self.displaySelectedHouseholdCharacteristics()
self.displayAvailablePersonalCharacteristics()
self.displaySelectedPersonalCharacteristics()
self.listDiets()
self.getCropTypes()
self.displayStandardOfLiving()
self.getExpenseItems()
self.getAgeRange(self.cmbAgeBottom, 0, 100)
self.getAgeRange(self.cmbAgeTop, 1, 101)
self.displayAvailableCrops()
self.displaySelectedCrops()
self.displayAvailableLivestock()
self.displaySelectedLivestock()
self.displayAvailableWildfoods()
self.displaySelectedWildfoods()
self.displayAvailableEmployment()
self.displaySelectedEmployment()
self.displayAvailableTransfers()
self.displaySelectedTransfers()
self.displayAvailableAssets()
self.displaySelectedAssets()
def importProject(self):
''' Import all projects '''
msg = "Are you sure you want to to import this project?"
ret = QMessageBox.question(self,"Confirm Import", msg, QMessageBox.Yes|QMessageBox.No)
# if import is rejected return without deleting
if ret == QMessageBox.No:
return
# import project
controller = Controller()
controller.importIhmProject( str(self.txtFilename.text()) )
# import message
QMessageBox.information(self,"Importing Data from OpenIHM file","Project data has been imported.")
self.close()
def saveHousehold(self):
''' Saves newly created household data to database '''
# get the data entered by user
hhid = self.txtShortHouseHoldName.text()
householdname = self.txtHouseholdName.text()
dateofcollection = self.dtpDateVisted.date().toString("yyyy-MM-dd")
pid = self.projectid
controller = Controller()
project = controller.getProject(pid)
project.addHousehold(hhid, householdname, dateofcollection)
# close new project window
self.parent.getHouseholds()
self.mdiClose()
def __init__(self, parent):
''' Set up the dialog box interface '''
QtGui.QDialog.__init__(self)
self.setupUi(self)
self.parent = parent
# get current project details
controller = Controller()
self.getCurrencies()
self.project = controller.getProject( self.parent.projectid )
self.getProjectData()
# allow the calendar widget to pop up
self.dtpStartDate.setCalendarPopup(True)
self.dtpEndDate.setCalendarPopup(True)
def findProject(self):
''' Find a project matching the criteria entered by user '''
pid = self.txtProjectID.text()
ptitle = self.txtProjectTitle.text()
controller = Controller()
if ( controller.existsProjectMatching( pid, ptitle ) ):
form = FrmFindProjectResults(self.parent, pid, ptitle)
subWin = self.parent.mdi.addSubWindow(form)
self.parent.centerSubWindow(subWin)
# close this form
self.parent.mdi.closeActiveSubWindow()
# show the details form
form.show()
else:
msg = "No project matching the criteria specified exists."
QMessageBox.information(self,"Find Project", msg)
def deleteProject(self):
''' Delete Selected Project '''
selectedRow = self.getCurrentRow(self.tblResults)
pid = self.tblResults.model().item(selectedRow,0).text()
ptitle = self.tblResults.model().item(selectedRow,1).text()
msg = "Are you sure you want to delete " + ptitle + " project?"
ret = QMessageBox.question(self,"Confirm Deletion", msg, QMessageBox.Yes|QMessageBox.No)
# if opening is rejected return
if ( ret == QMessageBox.No ):
return
controller = Controller()
controller.deleteProject(pid)
self.showResults()
def showResults(self):
''' Find a project matching the criteria entered by user '''
pid = self.txtProjectID.text()
ptitle = self.txtProjectTitle.text()
controller = Controller()
projects = controller.getProjectsMatching( pid, ptitle )
count = len( projects )
self.setWindowTitle("%i matching project(s) found." % (count) )
model = QStandardItemModel(1,2)
# set model headers
model.setHorizontalHeaderItem(0,QStandardItem('Project ID.'))
model.setHorizontalHeaderItem(1,QStandardItem('Project Title'))
model.setHorizontalHeaderItem(2,QStandardItem('Start Date'))
model.setHorizontalHeaderItem(3,QStandardItem('End Date'))
# add data rows
num = 0
for project in projects:
qtProjectID = QStandardItem( "%i" % project.getProjectID() )
qtProjectID.setTextAlignment( Qt.AlignCenter )
qtProjectTitle = QStandardItem( project.getProjectName() )
qtStartDate = QStandardItem( QDate( project.getStartDate() ).toString("dd/MM/yyyy") )
qtStartDate.setTextAlignment( Qt.AlignCenter )
qtEndDate = QStandardItem( QDate( project.getEndDate() ).toString("dd/MM/yyyy") ) if project.getEndDate() != None else QStandardItem( "")
qtEndDate.setTextAlignment( Qt.AlignCenter )
model.setItem( num, 0, qtProjectID )
model.setItem( num, 1, qtProjectTitle )
model.setItem( num, 2, qtStartDate )
model.setItem( num, 3, qtEndDate )
num = num + 1
self.tblResults.setModel(model)
self.tblResults.resizeColumnsToContents()
self.tblResults.show()
def moveSelectedHouseholdCharacteristics(self):
''' Add selected available household characteristics to Project'''
numSelected = self.countRowsSelected(self.tblAvailableHouseholdCharacteristics)
if numSelected != 0:
selectedRows = self.getSelectedRows(self.tblAvailableHouseholdCharacteristics)
for row in selectedRows:
charname = self.tblAvailableHouseholdCharacteristics.model().item(row,0).text()
if not self.project.existsProjectCharacteristic(charname):
controller = Controller()
char = controller.getGlobalCharacteristic(charname)
self.project.addProjectCharacteristic(char.name, char.chartype, char.datatype)
else:
msg = "The household characteristic labelled, %s, has already been added to project" % (charname)
QMessageBox.information(self,"Project Configuration",msg)
self.displaySelectedHouseholdCharacteristics()
else:
msg = "Please select the household characteristics you want to add."
QMessageBox.information(self,"Project Configuration",msg)
def saveCharacteristic(self):
''' Saves characteristic '''
if self.cboYesNoVal.isVisible():
charVal = self.cboYesNoVal.currentText()
else:
charVal = self.txtValue.text()
controller = Controller()
project = controller.getProject(self.pid)
household = project.getHousehold(self.hhid)
if not household.existsCharacteristic(self.charName):
household.addCharacteristic(self.charName, charVal)
else:
char = household.getCharacteristic(self.charName)
char.editData(self.charName, charVal)
# close window and refresh characteristics
self.parent.retrieveHouseholdCharacteristics()
self.mdiClose()
def saveCharacteristic(self):
controller = Controller()
charname = self.txtCharacteristic.text()
datatype = self.getDataType( self.cmbDataType.currentText() )
description = self.txtDescription.text()
if self.characteristic == "":
if controller.existsGlobalCharacteristic( charname ):
QMessageBox.information(self,"Add Personal Characteristic","Personal Characteristic Already Exists.")
return
else:
controller.addGlobalCharacteristic(charname, "Personal", datatype, description)
else:
char = controller.getGlobalCharacteristic( self.characteristic )
char.editData(charname, "Personal", datatype, description)
self.parent.loadCharacteristics()
self.close()
def run():
# parse arguments
args = arg_parser.parse()
# parse game file
game_rules = game_parser.parse(args.game)
# create game
game = game_builder.build_game(game_rules)
# create view
if args.debug:
view = DebugView(args.log) if args.log else DebugView()
else:
print(args.log)
view = PrettyView(args.log) if args.log else PrettyView()
# start game loop
game_controller = Controller(game, view)
game_controller.run()
def saveProject(self):
''' Saves newly created data to database '''
# get the data entered by user
projectname = self.txtProjectName.text()
startdate = self.dtpStartDate.date().toString("yyyy-MM-dd")
enddate = self.dtpEndDate.date().toString("yyyy-MM-dd")
description = self.txtDescription.toPlainText()
currency = self.cmbCurrency.currentText()
# create project
controller = Controller()
project = controller.addProject(projectname, startdate, enddate, description, currency)
# set the newly inserted project as the current project
self.parent.projectid = project.getProjectID()
self.parent.projectname = project.getProjectName()
self.parent.setWindowTitle("Open IHM - " + projectname)
self.parent.actionClose_Project.setDisabled(False)
# close new project window
self.mdiClose()
def importAll(self):
''' Import all projects '''
msg = "Are you sure you want to to import all listed projects?"
ret = QMessageBox.question(self,"Confirm Import", msg, QMessageBox.Yes|QMessageBox.No)
# if import is rejected return without deleting
if ret == QMessageBox.No:
return
selectedProjects = []
# get all project ids as selected items
for row in range(0, self.tblProjects.model().rowCount()) :
selectedProjects.append( self.tblProjects.model().item(row,0).text() )
# import projects
controller = Controller()
controller.importProjects( str(self.txtFilename.text()), selectedProjects )
# import message
QMessageBox.information(self,"Importing Data from Access","All projects in the database have been imported.")
self.close()
class Pipeline:
"""
执行的总pipeline main loop，在这里初始化各个感知模块，并运行控制模块及其与感知的交互
"""
def __init__(self):
self._init_component()
def __del__(self):
logger.info('auto drive pipeline finished...')
def _init_component(self):
"""
初始化各组件
:return:
"""
self.frame_catcher = FrameCatcher()
self.percepter = Percepter()
self.controller = Controller()
def _sensing(self):
sensor = SensorData()
frame = self.frame_catcher.loop_catch()
sensor.main_camera = frame
return sensor
def run(self):
logger.info('pipeline start running main loop...')
while True:
sensor = self._sensing()
state = self.percepter.run(sensor)
self.controller.react(state)
cv2.imshow('data frame',
cv2.cvtColor(sensor.main_camera, cv2.COLOR_BGR2RGB))
if cv2.waitKey(25) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
def delCharacteristic(self):
numSelected = self.countRowsSelected(self.tblHouseholdCharacteristics)
if numSelected != 0:
# confirm deletion
if numSelected == 1:
msg = "Are you sure you want to delete the selected characteristic?"
else:
msg = "Are you sure you want to delete the selected characteristicss?"
ret = QMessageBox.question(self,"Confirm Deletion", msg, QMessageBox.Yes|QMessageBox.No)
# if deletion is rejected return without deleting
if ret == QMessageBox.No:
return
# delete selected characteristics
controller = Controller()
selectedRows = self.getSelectedRows(self.tblHouseholdCharacteristics)
for row in selectedRows:
charname = self.tblHouseholdCharacteristics.model().item(row,0).text()
controller.delGlobalCharacteristic(charname)
self.loadCharacteristics()
else:
QMessageBox.information(self,"Delete Characteristics","Please select the rows containing characteristics to be deleted.")
def script():
"""
执行脚本：data/source/script.txt
:return:
"""
controller = Controller()
for i in range(2)[::-1]:
print(i + 1)
time.sleep(1)
with open(
os.path.join(os.sep.join(__file__.split(os.sep)[:-1]), 'data',
'source', 'script.txt'), 'r') as f:
moves = [
x.strip() for x in f.readlines()
if x.strip() in ['left', 'right', 'forward', 'backward', 'stop']
]
previous, current = None, None
while True:
for m in moves:
try:
current = np.array(ImageGrab.grab(bbox=(0, 40, 800, 640)))
controller.react_basic(m)
print('performing: %s' % m)
flow = processor.optical_flow(previous, current)
cv2.imshow('window', cv2.cvtColor(flow, cv2.COLOR_BGR2RGB))
except Exception as e:
print(e)
traceback.print_exc()
finally:
previous = current
time.sleep(0.1)
if cv2.waitKey(25) & 0xFF == ord('q'):
cv2.destroyAllWindows()
return
def displayAvailableHouseholdCharacteristics(self):
''' Retrieve and display available Household Characteristics '''
controller = Controller()
chars = controller.getGlobalCharacteristics("Household")
model = QStandardItemModel(1,2)
# set model headers
model.setHorizontalHeaderItem(0,QStandardItem('Characteristic'))
model.setHorizontalHeaderItem(1,QStandardItem('Data Type'))
# add data rows
num = 0
for char in chars:
qtCharacteristic = QStandardItem( char.name )
qtDataType = QStandardItem( char.datatypestr )
model.setItem( num, 0, qtCharacteristic )
model.setItem( num, 1, qtDataType )
num = num + 1
self.tblAvailableHouseholdCharacteristics.setModel(model)
self.tblAvailableHouseholdCharacteristics.setSelectionMode(QAbstractItemView.ExtendedSelection)
self.tblAvailableHouseholdCharacteristics.resizeColumnsToContents()
def main():
# Sort out the logging config
logging.basicConfig(format='%(asctime)s %(levelname)s:%(message)s',
level="DEBUG")
# Should we fake all sorts of things? (no db/no serial/no shutdown/no relay)
demo_mode = False
# Spin up our controller - this handles everything
controller_obj.init()
controller_obj.controller_obj = Controller()
controller_obj.controller_obj.start(demo_mode)
# We just need this to stop the main thread from completing
while True:
logging.info("Server is just fine & dandy!")
time.sleep(10)
if __name__ == "__main__":
# Initialise the ip
ip = 'localhost' # local testing
#ip = "169.254.246.235" # ROV pi
#ip = "169.254.231.182" # Kacper's personal pi
# Inform that the initialisation phase has started
print("Loading...")
# Clear the cache on start
dm.clear()
# Build the controller's instance
controller = Controller()
# Initialise the server connection
connection = Connection(ip=ip, port=50000)
# Initialise the port iterator
port = 50010
# Initialise the video streams
streams = [
VideoStream(ip=ip, port=p) for p in range(port, port + CAMERAS_COUNT)
]
# Inform that the execution phase has started
print("Starting tasks...")
def __init__(self):
self.contorller = Controller()
self.detector = Detector()
self.pre_img_id = -1
self.cur_img_id = -1
def getCurrencies(self):
''' Loads currencies into the currency combo box '''
controller = Controller()
currencies = controller.getCurrencies()
for currency in currencies:
self.cmbCurrency.addItem(currency.name)
def main(connection_string, target_altitude):
"""Takes the drone up and then lands."""
# Setup logging
logger = logging.getLogger('control')
logger.setLevel(logging.DEBUG)
filehandler = logging.FileHandler('simpletakeoff.log')
filehandler.setLevel(logging.DEBUG)
console = logging.StreamHandler()
console.setLevel(logging.DEBUG)
formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
filehandler.setFormatter(formatter)
console.setFormatter(formatter)
logger.addHandler(filehandler)
logger.addHandler(console)
# Connect to flight controller
logger.debug("Starting program, attempting connection to flight controller.")
vehicle_control = Controller(connection_string, baud=57600)
if not SIMULATION:
time.sleep(60) # Sleep to avoid immediate takeoff on boot
# Arm and takeoff
logger.debug("Arming...")
vehicle_control.arm()
vehicle_control.takeoff(target_altitude)
# Give time for user controller to takeover (if you desire to test controller takeover)
logger.debug("Hovering for 30 seconds...")
for second in range(30):
vehicle_control.check_geofence(10, target_altitude + 10)
if vehicle_control.vehicle.mode.name != "GUIDED":
break
time.sleep(1)
# Land if still in guided mode (i.e. no user takeover, no flight controller failure)
if vehicle_control.vehicle.mode.name == "GUIDED":
logger.debug("Landing...")
vehicle_control.land()
# Always keep the programming running and logging until the vehicle is disarmed
while vehicle_control.vehicle.armed:
vehicle_control.log_flight_info()
time.sleep(3)
logger.debug("Finshed program.")
sys.exit(0)
def showDetails(self):
controller = Controller()
char = controller.getGlobalCharacteristic(self.characteristic)
self.txtCharacteristic.setText( char.name )
self.txtDescription.setText( char.description )
self.cmbDataType.setCurrentIndex( self.cmbDataType.findText( char.datatypestr ) )
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtCore import QThread, pyqtSignal
from view.view import TrackNPredView
from control.controller import Controller
from model.model import TnpModel
if __name__ == "__main__":
import sys
app = QtWidgets.QApplication(sys.argv)
Dialog = QtWidgets.QDialog()
controller = Controller()
view = TrackNPredView()
## to get attributes
view.setupUi(Dialog)
model = TnpModel(controller)
controller.setView(view)
controller.setModel(model)
#show ui
Dialog.show()
sys.exit(app.exec_())
class Car(object):
""" Offline car-control, with only one thread.
"""
def __init__(self):
self.contorller = Controller()
self.detector = Detector()
self.pre_img_id = -1
self.cur_img_id = -1
@staticmethod
def unpackage_paras(packaged_parameters):
""" Unpackage the parameters.
@Paras:
packaged_parameters: np array
@Returns:
distance_to_center
"""
cur_paras = packaged_parameters[0:13]
w_left, w_right, w_middle = cur_paras[0:3], cur_paras[3:6], cur_paras[6:9]
distance_to_center = cur_paras[9]
distance_at_middle = cur_paras[10]
radian = cur_paras[11]
curvature = cur_paras[12]
stop_signal = (np.all(w_left == np.zeros(3)) and np.all(w_right == np.zeros(3)))
return distance_to_center, distance_at_middle, curvature, stop_signal
@staticmethod
def unpackage_paras_from_buffer(buffer):
""" Unpackage the parameters from buffer.
@Paras:
buffer: str
The recv buffer.
Note that the default recv size should be 112 (np.array(13, dtype=float64))
@Returns:
distance_to_tangent
angle_of_tangent
"""
num_of_paras = floor(len(buffer) / 128)
packaged_parameters = np.frombuffer(buffer, dtype=np.float64).reshape(int(16 * num_of_paras))
if len(packaged_parameters) < 16:
return -1, 0, 0, False
cur_paras = packaged_parameters[0:16]
image_id = int(cur_paras[0])
w_left, w_right = cur_paras[1:4], cur_paras[4:7]
distance_to_tangent = cur_paras[14]
angle_of_tangent = cur_paras[15]
stop_signal = (np.all(w_left == np.zeros(3)) and np.all(w_right == np.zeros(3)))
return image_id, distance_to_tangent, angle_of_tangent, stop_signal
def send_images(self, connection, stream):
""" Send images. Single thread, will block.
Helper function for online mode.
"""
connection.write(struct.pack('<L', stream.tell()))
connection.flush()
stream.seek(0)
connection.write(stream.read())
stream.seek(0)
stream.truncate()
def recv_parameters(self, client_socket):
""" Receive parameters from the server. Single thread, will block.
Helper function for online mode.
"""
buffer = client_socket.recv(1024)
if (buffer is not None):
img_id, d2t, aot, stop_signal = Car.unpackage_paras_from_buffer(buffer)
if img_id <= self.pre_img_id:
return
self.cur_img_id = img_id
if stop_signal:
self.contorller.finish_control()
else:
self.contorller.make_decision_with_policy(1, d2t, aot)
self.pre_img_id = img_id
def run_offline(self, debug=True):
stream = io.BytesIO()
first_start = True
waitting_for_ob = configs['avoid_collision']
with picamera.PiCamera(resolution='VGA') as camera:
with io.BytesIO() as stream:
for _ in camera.capture_continuous(stream, format='jpeg', use_video_port=True):
stream.seek(0)
ori_image = img_process.img_load_from_stream(stream)
# ------------- preprocessing -------------
debug_img = img_process.crop_image(ori_image, 0.45, 0.85)
debug_img = img_process.down_sample(debug_img, (160, 48))
image = img_process.binarize(debug_img)
# -----------------------------------------
# st = time.time()
paras = self.detector.get_wrapped_all_parameters(image)
dc, dm, cur, ss = Car.unpackage_paras(paras)
dis_2_tan, pt = Detector.get_distance_2_tan(paras[6:9])
radian_at_tan = atan(paras[14])
# print('detect time.:', time.time() - st)
if waitting_for_ob:
ob = img_process.detect_obstacle(ori_image)
# display the fitting result in real time
if configs['debug']:
# ------------- 1. display fitting result on the fly -------------
debug_img = img_process.compute_debug_image(debug_img, IMG_W, IMG_H, NUM_OF_POINTS, pt, paras)
img_process.show_img(debug_img)
# ----------------------------------------------------------------
if first_start:
self.contorller.start()
first_start = False
# Control the car according to the parameters
if waitting_for_ob and ob:
ob = False
print("attampting to avoid ...")
# self.contorller.collision_avoid(time.time())
waitting_for_ob = False
else:
# st = time.time()
## 1. ADP
# self.contorller.make_decision_with_policy(1, dis_2_tan, radian_at_tan)
## 2. pure pursuit
# l_d, sin_alpha = Detector.get_distance_angle_pp(paras[6:9])
# self.contorller.make_decision_with_policy(2, l_d, sin_alpha)
## 3. Car following with ADP
d_arc = img_process.detect_distance(ori_image)
print(d_arc)
# self.contorller.make_decision_with_policy(3, dis_2_tan, radian_at_tan, d_arc)
# print('decision time: ', time.time() - st)
## 4. Car followings on stright lane
# self.contorller.make_decision_with_policy(4, d_arc)
## 5. Coupled controller - Car following
self.contorller.make_decision_with_policy(5, d_arc, dis_2_tan, radian_at_tan)
stream.seek(0)
stream.truncate()
def run_online(self, ip, port):
pass
def run_online_single(self, ip, port):
client_socket = socket.socket()
client_socket.connect((ip, port))
connection = client_socket.makefile('wb')
first_start = True
with picamera.PiCamera() as camera:
camera.resolution = (640, 480)
camera.framerate = 30
time.sleep(1)
stream = io.BytesIO()
for _ in camera.capture_continuous(stream, 'jpeg', use_video_port=True):
# start_time = time.time()
self.send_images(connection, stream)
self.recv_parameters(client_socket)
if first_start:
self.contorller.start()
first_start = False
# print('processed img ' + str(self.cur_img_id), time.time() - start_time)
connection.write(struct.pack('<L', 0))
connection.close()
client_socket.close()
def run_as_fast_as_you_can(self):
self.contorller.start()
def stop(self):
self.contorller.finish_control()
self.contorller.cleanup()
def main(target_altitude, radius):
"""Takes the drone up and then lands."""
# Setup logging
logger = logging.getLogger('control')
logger.setLevel(logging.DEBUG)
filehandler = logging.FileHandler('simpleflight.log')
filehandler.setLevel(logging.DEBUG)
console = logging.StreamHandler()
console.setLevel(logging.DEBUG)
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
filehandler.setFormatter(formatter)
console.setFormatter(formatter)
logger.addHandler(filehandler)
logger.addHandler(console)
# Connect to the drone
logger.debug(
"Starting program, attempting connection to flight controller.")
vehicle_control = Controller(CONNECTION_STRING, baud=57600)
# Sleep to avoid immediate takeoff on boot
if not SIMULATION:
time.sleep(60)
# Arm and takeoff
logger.debug("Arming...")
vehicle_control.arm()
vehicle_control.takeoff(target_altitude)
# Create points
home_gps = location_global_relative_to_gps_reading(vehicle_control.home)
north_gps = get_location_offset(home_gps, radius, 0)
south_gps = get_location_offset(home_gps, -1 * radius, 0)
east_gps = get_location_offset(home_gps, 0, radius)
west_gps = get_location_offset(home_gps, 0, -1 * radius)
points = [north_gps, south_gps, east_gps, west_gps, home_gps]
# Transform GpsReading to LocationGlobalRelative
for index, point in enumerate(points):
points[index] = gps_reading_to_location_global(point)
logger.debug("Destination {}: {}".format(index, points[index]))
# Go to the points
if vehicle_control.vehicle.mode.name == "GUIDED":
logger.debug("Flying to points...")
for point in points:
if vehicle_control.vehicle.mode.name != "GUIDED":
break
vehicle_control.goto(point)
# Wait for vehicle to reach destination before updating the point
for sleep_time in range(10):
if vehicle_control.vehicle.mode.name != "GUIDED":
break
vehicle_control.log_flight_info(point)
# Don't let the vehicle go too far (could be stricter if get_distance
# improved and if gps was more accurate. Also note that altitude
# is looser here to avoid false landings (since gps altitude not
# accurate at all).
vehicle_control.check_geofence(radius * 2,
target_altitude + 20)
current = vehicle_control.vehicle.location.global_relative_frame
current_reading = location_global_relative_to_gps_reading(
current)
point_reading = location_global_relative_to_gps_reading(point)
distance = get_distance(current_reading, point_reading)
if distance < 1:
logger.debug('Destination Reached')
time.sleep(3)
break
time.sleep(3)
# Land if still in guided mode (i.e. no user takeover, no flight controller failure)
if vehicle_control.vehicle.mode.name == "GUIDED":
logger.debug("Landing...")
vehicle_control.land()
# Always keep the programming running and logging until the vehicle is disarmed
while vehicle_control.vehicle.armed:
vehicle_control.log_flight_info()
time.sleep(3)
logger.debug("Finshed program.")
sys.exit(0)
def main():
"""Takes the drone up and then lands."""
# Setup logging
logger = logging.getLogger('control')
logger.setLevel(logging.DEBUG)
filehandler = logging.FileHandler('main.log')
filehandler.setLevel(logging.DEBUG)
console = logging.StreamHandler()
console.setLevel(logging.DEBUG)
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
filehandler.setFormatter(formatter)
console.setFormatter(formatter)
logger.addHandler(filehandler)
logger.addHandler(console)
# Connect to xBee
com = Communication(COM_CONNECTION_STRING, 0.1)
logger.debug("Connected to wireless communication receiver")
com.send(u"Connected to wireless communication receiver")
# Connect to i2c data server
data_client = I2cDataClient("tcp://localhost:5555")
if not data_client.read():
logger.critical("Can't connect to zmq data server")
com.send(u"Can't connect to zmq data server")
sys.exit(1)
# Connect to the drone
logger.debug(
"Starting program, attempting connection to flight controller.")
com.send(u"Starting program, attempting connection to flight controller")
vehicle_control = None
try:
vehicle_control = Controller(PIXHAWK_CONNECTION_STRING,
baud=57600,
com=com)
logger.debug("Connected to flight controller")
com.send(u"Connected to flight controller")
except dronekit.APIException as err:
logger.critical("dronekit.APIException: {}".format(err))
logger.critical("Could not connect to flight controller.")
com.send(u"Could not connect to flight controller.")
sys.exit(1)
# Wait until the waypoints flight path is received from GCS
logger.debug("Waiting to receive flight path from GCS")
com.send(u"Waiting to receive flight path from GCS")
waypoints = com.receive()
while not waypoints:
waypoints = com.receive()
time.sleep(1)
# Create points
start_location = vehicle_control.vehicle.location.global_relative_frame
points = create_waypoints(logger, com, start_location, waypoints)
if not points:
logger.critical("Invalid points received from GCS")
com.send(u"Invalid points received from GCS")
sys.exit(1)
# Arm and takeoff
vehicle_control.arm()
vehicle_control.takeoff(10)
points.append(vehicle_control.home)
# Log points
for index, point in enumerate(points):
logger.debug("Destination {}: {}".format(index, point))
# Go to the points
flight_start_time = time.time()
if vehicle_control.vehicle.mode.name == "GUIDED":
logger.debug("Flying to points...")
for point in points:
com.send(u"Destination: {}".format(point))
if vehicle_control.vehicle.mode.name != "GUIDED":
com.send(u"Mode no longer guided")
break
vehicle_control.goto(point)
# Wait for vehicle to reach destination before updating the point
for sleep_time in range(60):
if vehicle_control.vehicle.mode.name != "GUIDED":
com.send(u"Mode no longer guided")
break
vehicle_control.log_flight_info(point)
data_for_gcs = package_data(
vehicle_control.home,
vehicle_control.vehicle.location.global_relative_frame,
data_client,
time.time() - flight_start_time)
com.send(data_for_gcs)
# Don't let the vehicle go too far (could be stricter if get_distance
# improved and if gps was more accurate. Also note that altitude
# is looser here to avoid false landings (since gps altitude not
# accurate at all).
vehicle_control.check_geofence(MAX_RADIUS + 10,
MAX_ALTITUDE + 10)
if is_destination_reached(logger, com, vehicle_control, point):
break
time.sleep(1)
# Land if still in guided mode (i.e. no user takeover, no flight controller failure)
if vehicle_control.vehicle.mode.name == "GUIDED":
vehicle_control.land()
# Always keep the programming running and logging until the vehicle is disarmed
while vehicle_control.vehicle.armed:
vehicle_control.log_flight_info()
time.sleep(1)
# Program end
logger.debug("Finished program.")
com.send("Finished program.")
sys.exit(0)
