From 9916fbc46fb86705ba71f19862b73bbad037040f Mon Sep 17 00:00:00 2001 From: uavteambyu Date: Tue, 5 Dec 2017 08:38:32 -0700 Subject: [PATCH] Added waypoint_planning feature to tuning branch for pull request. --- .../waypoint_planning/CMakeLists.txt | 28 ++ .../waypoint_planning/README.md | 22 ++ .../launch/waypoint_planning.launch | 5 + .../waypoint_planning/package.xml | 30 ++ .../waypoint_planning/plugin.xml | 17 + .../waypoint_planning/resource/MyPlugin.ui | 298 ++++++++++++++++++ .../resource/WaypointGrid.ui | 19 ++ .../scripts/waypoint_planning | 9 + .../waypoint_planning/setup.py | 12 + .../src/waypoint_planning/__init__.py | 0 .../waypoint_planning/waypoint_planning.py | 254 +++++++++++++++ 11 files changed, 694 insertions(+) create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/CMakeLists.txt create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/README.md create mode 100644 rosplane_utils/src/waypoint_planning/waypoint_planning/launch/waypoint_planning.launch create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/package.xml create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/plugin.xml create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/resource/MyPlugin.ui create mode 100644 rosplane_utils/src/waypoint_planning/waypoint_planning/resource/WaypointGrid.ui create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/scripts/waypoint_planning create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/setup.py create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/__init__.py create mode 100755 rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/waypoint_planning.py diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/CMakeLists.txt b/rosplane_utils/src/waypoint_planning/waypoint_planning/CMakeLists.txt new file mode 100755 index 0000000..9d07d00 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 2.8.3) +project(waypoint_planning) + +find_package(catkin REQUIRED COMPONENTS + rospy + rqt_gui + rqt_gui_py + ) + + +# To enable assertions when compiled in release mode. +catkin_package() +catkin_python_setup() + +include_directories( + ${catkin_INCLUDE_DIRS} +) + + +install(PROGRAMS scripts/waypoint_planning + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +install(DIRECTORY launch resource + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) +install(FILES plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/README.md b/rosplane_utils/src/waypoint_planning/waypoint_planning/README.md new file mode 100755 index 0000000..d2d61c5 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/README.md @@ -0,0 +1,22 @@ +ROSplane Plugins +================= + +This ROS package contains basic plugins for controlling a uav, setting waypoints etc.. + +The waypoint planning node allows dynamic changes (without rewriting +code or rerunning nodes) of the waypoint path. + +It's functionality is best observed by opening Gazebo and launching the +rosplane groundstation. + +As shown in the GUI, the points listed on the right represent the +current waypoints the plane is/will attempt to reach. Clicking the clear +list button clears the plane's current waypoints completely and returns the plane to +loiter about the origin. + +On the left are waypoints to be sent to the plane. These can be added +and edited using the text boxes below, but will not be executed by the +plane until sent to the waypoint list. + +Finally, waypoint lists from the left column can be saved and loaded +for later use using the corresponding buttons. \ No newline at end of file diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/launch/waypoint_planning.launch b/rosplane_utils/src/waypoint_planning/waypoint_planning/launch/waypoint_planning.launch new file mode 100644 index 0000000..6b27604 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/launch/waypoint_planning.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/package.xml b/rosplane_utils/src/waypoint_planning/waypoint_planning/package.xml new file mode 100755 index 0000000..77e087e --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/package.xml @@ -0,0 +1,30 @@ + + waypoint_planning + 2.0.2 + The waypoint_planning package + + Tim Whiting + + Tim Whiting + + BSD + + catkin + + + python_qt_binding + qt_gui + rospy + rqt_gui + rqt_gui_py + python-rospkg + roslib + rosnode + rqt_py_common + rosplane_msgs + + + + + + diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/plugin.xml b/rosplane_utils/src/waypoint_planning/waypoint_planning/plugin.xml new file mode 100755 index 0000000..b993f6f --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/plugin.xml @@ -0,0 +1,17 @@ + + + + An example Python GUI plugin to create a great user interface. + + + + + folder + Plugins related to the AUVSI competition. + + + system-help + Great user interface to provide real value. + + + diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/resource/MyPlugin.ui b/rosplane_utils/src/waypoint_planning/waypoint_planning/resource/MyPlugin.ui new file mode 100755 index 0000000..e91b310 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/resource/MyPlugin.ui @@ -0,0 +1,298 @@ + + + WaypointPlanner + + + Qt::NonModal + + + + 0 + 0 + 1000 + 1000 + + + + Waypoint Planner + + + + + 0 + 0 + 1000 + 25 + + + + + Menu + + + + + + + + 0 + 0 + 3 + 22 + + + + + + + 0 + 30 + 891 + 511 + + + + + 5 + + + + + 5 + + + + + + 0 + 0 + + + + + 400 + 500 + + + + true + + + + + 0 + 0 + 385 + 454 + + + + + + 0 + 0 + 381 + 458 + + + + + + + + 0 + 260 + + + + + + + + + + + 0 + 0 + + + + Add Waypoint + + + + + + + + 0 + 0 + + + + Delete Waypoint + + + + + + + + + + 0 + 0 + + + + + 320 + 0 + + + + Qt::Horizontal + + + + + + + QLayout::SetDefaultConstraint + + + 5 + + + + + + + + + + + + + + + + + + + + + 16777215 + 25 + + + + Location: x,y,z + + + + + + + + 16777215 + 25 + + + + Orientation: theta + + + + + + + + 16777215 + 25 + + + + Air Velocity + + + + + + + + + + + + + + + + true + + + -> + + + + + + + true + + + + + 0 + 0 + 385 + 454 + + + + + + 0 + 0 + 381 + 451 + + + + + + + + + + + + + + Save File + + + + + + + Load File + + + + + + + Clear List + + + + + + + + + + + diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/resource/WaypointGrid.ui b/rosplane_utils/src/waypoint_planning/waypoint_planning/resource/WaypointGrid.ui new file mode 100644 index 0000000..3931bcf --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/resource/WaypointGrid.ui @@ -0,0 +1,19 @@ + + + Form + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/scripts/waypoint_planning b/rosplane_utils/src/waypoint_planning/waypoint_planning/scripts/waypoint_planning new file mode 100755 index 0000000..74c12d7 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/scripts/waypoint_planning @@ -0,0 +1,9 @@ +#!/usr/bin/env python +import sys + +from waypoint_planning.waypoint_planning import WaypointPlanner +from rqt_gui.main import Main + +plugin = 'waypoint_planning.waypoint_planning.WaypointPlanner' +main = Main(filename=plugin) +sys.exit(main.main(standalone=plugin)) diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/setup.py b/rosplane_utils/src/waypoint_planning/waypoint_planning/setup.py new file mode 100755 index 0000000..0390bf5 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['waypoint_planning'], + package_dir={'': 'src'}, + scripts={'scripts/waypoint_planning'} +) + +setup(**d) \ No newline at end of file diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/__init__.py b/rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/waypoint_planning.py b/rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/waypoint_planning.py new file mode 100755 index 0000000..eef2a64 --- /dev/null +++ b/rosplane_utils/src/waypoint_planning/waypoint_planning/src/waypoint_planning/waypoint_planning.py @@ -0,0 +1,254 @@ +import os +import rospy +import rospkg +import math + +from qt_gui.plugin import Plugin +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget, QPushButton, QListWidget, QLineEdit, QFileDialog +from python_qt_binding.QtCore import QRegExp +from python_qt_binding.QtGui import QRegExpValidator +import json +import os +from parse import parse,compile +import csv +from rosplane_msgs.msg import Waypoint + + + +class WaypointPlanner(Plugin): + + def __init__(self, context): + super(WaypointPlanner, self).__init__(context) + # Give QObjects reasonable names + self.setObjectName('WaypointPlanner') + + # Process standalone plugin command-line arguments + from argparse import ArgumentParser + parser = ArgumentParser() + # Add argument(s) to the parser. + parser.add_argument("-q", "--quiet", action="store_true", + dest="quiet", + help="Put plugin in silent mode") + args, unknowns = parser.parse_known_args(context.argv()) + if not args.quiet: + print 'arguments: ', args + print 'unknowns: ', unknowns + + # Create QWidget + self._widget = QWidget() + # Get path to UI file which should be in the "resource" folder of this package + ui_file = os.path.join(rospkg.RosPack().get_path('waypoint_planning'), 'resource', 'MyPlugin.ui') + # Extend the widget with all attributes and children from UI file + loadUi(ui_file, self._widget) + # Give QObjects reasonable names + self._widget.setObjectName('Waypoint Planner') + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) + # Add widget to the user interface + context.add_widget(self._widget) + # Buttons + self.clearWaypointsButton = self._widget.findChild(QPushButton,"clearWaypointsButton") + self.clearWaypointsButton.clicked.connect(self.handleClearWaypoints) + self.loadFileButton = self._widget.findChild(QPushButton,"loadFileButton") + self.loadFileButton.clicked.connect(self.handleLoadFile) + self.saveFileButton = self._widget.findChild(QPushButton,"saveFileButton") + self.saveFileButton.clicked.connect(self.handleSaveFile) + self.addWaypointButton = self._widget.findChild(QPushButton,"addWaypointButton") + self.addWaypointButton.clicked.connect(self.handleAddWaypoint) + self.deleteWaypointButton = self._widget.findChild(QPushButton,"deleteWaypointButton") + self.deleteWaypointButton.clicked.connect(self.handleDeleteWaypoint) + self.sendWaypointsButton = self._widget.findChild(QPushButton,"sendWaypointsButton") + self.sendWaypointsButton.clicked.connect(self.handleSendWaypoints) + #Text Edits + self.locationLineEdit = self._widget.findChild(QLineEdit,"locationLineEdit") + self.locationLineEdit.setText("50,50,-50") + regexp = QRegExp("\d+,\d+,-\d+") + validator = QRegExpValidator(regexp) + self.locationLineEdit.setValidator(validator) + regexp1 = QRegExp("\d+") + validator1 = QRegExpValidator(regexp1) + self.locationLineEdit.textChanged.connect(self.locationChanged) + self.orientationLineEdit = self._widget.findChild(QLineEdit,"orientationLineEdit") + self.orientationLineEdit.setText("45") + self.orientationLineEdit.setValidator(validator1) + self.orientationLineEdit.textChanged.connect(self.orientationChanged) + self.velocityLineEdit = self._widget.findChild(QLineEdit,"velocityLineEdit") + self.velocityLineEdit.setText("16") + self.velocityLineEdit.setValidator(validator1) + self.velocityLineEdit.textChanged.connect(self.velocityChanged) + #Lists + self.newWaypointList = self._widget.findChild(QListWidget,"newWaypointList") + self.currentWaypointList = self._widget.findChild(QListWidget,"currentWaypointList") + self.waypoints = [] + self.currentWaypoints = [] + self.currentWaypoint = None + self.newWaypointList.itemClicked.connect(self.newWaypointSelected) + self.fileDialog = QFileDialog() + self.fileDialog.setFileMode(QFileDialog.AnyFile) + #Publisher/Subscriber + self.publisher = rospy.Publisher('waypoint_path',Waypoint,queue_size=20) + self.subscriber = rospy.Subscriber('waypoint_path',Waypoint,self.publishedWaypointCallback) + + def updateWaypoint(self): + self.newWaypointList.item(self.waypoints.index(self.currentWaypoint)).setText(self.waypointToString(self.currentWaypoint)) + + def locationChanged(self,text): + try: + (x,y,z) = parse('{},{},{}',text) + if self.currentWaypoint is not None and x is not None and y is not None and z is not None and z is not "-": + self.currentWaypoint.w[0] = float(x) + self.currentWaypoint.w[1] = float(y) + self.currentWaypoint.w[2] = float(z) + self.updateWaypoint() + except: + print("Invalid location") + + def orientationChanged(self,text): + if self.currentWaypoint is not None and text is not None and text is not "": + try: + temp = float(text)*2*math.pi/360 #convert to radians + self.currentWaypoint.chi_d = temp + self.updateWaypoint() + except: + print("invalid orientation string") + + + def velocityChanged(self,text): + if self.currentWaypoint is not None and text is not "": + self.currentWaypoint.Va_d = float(text) + self.updateWaypoint() + + def waypointToString(self,waypoint): + return "Waypoint X:{} Y:{} Z:{} @{}degrees, {}kph".format(waypoint.w[0],waypoint.w[1],waypoint.w[2],waypoint.chi_d*180/math.pi,waypoint.Va_d) + + def createWaypoint(self,waypoint): + self.waypoints.append(waypoint) + self.newWaypointList.addItem(self.waypointToString(waypoint)) + + def newWaypointSelected(self,item): + self.currentWaypoint = self.waypoints[self.newWaypointList.currentRow()] + + def handleClearWaypoints(self): + roswaypoint = Waypoint() + roswaypoint.w[0] = 0.0 + roswaypoint.w[1] = 0.0 + roswaypoint.w[2] = 0.0 + roswaypoint.chi_d = 0.0 + roswaypoint.chi_valid = False + roswaypoint.Va_d = 0.0 + roswaypoint.clear_wp_list = True + roswaypoint.set_current = True + self.publisher.publish(roswaypoint) + + + def handleLoadFile(self): + file_name = self.fileDialog.getOpenFileName(self._widget,'Open File', '/home/',"Waypoint Files (*.wp)") + if not file_name[0]: + return + print("OpenFileName:{}".format(file_name[0])) + f = open(file_name[0],'r') + csvReader = csv.reader(f) + self.waypoints = [] + for row in csvReader: + print(row) + waypoint = Waypoint() + waypoint.w[0] = float(row[0]) + waypoint.w[1] = float(row[1]) + waypoint.w[2] = float(row[2]) + temp = float(row[3])*2*math.pi/360 + waypoint.chi_d = temp + waypoint.Va_d = float(row[4]) + self.waypoints.append(waypoint) + f.close() + self.newWaypointList.clear() + for waypoint in self.waypoints: + self.newWaypointList.addItem(self.waypointToString(waypoint)) + + + def handleSaveFile(self): + file_name = self.fileDialog.getSaveFileName(self._widget,'Save File', '/home/',"Waypoint Files (*.wp)") + if not file_name[0]: + return + print("SaveFileName:{}".format(file_name[0])) + try: + f = open(file_name[0],'w') + except: + print("Could not save file. Permissions may have been denied") + return + csvWriter = csv.writer(f) + for waypoint in self.waypoints: + csvWriter.writerow([waypoint.w[0],waypoint.w[1],waypoint.w[2],waypoint.chi_d*180/math.pi,waypoint.Va_d]) + f.close() + + + def handleAddWaypoint(self): + text = self.locationLineEdit.text() + (x,y,z) = parse('{},{},{}',text) + orientation = self.orientationLineEdit.text() + velocity = self.velocityLineEdit.text() + waypoint = Waypoint() + waypoint.w[0] = float(x) + waypoint.w[1] = float(y) + waypoint.w[2] = float(z) + temp = float(orientation)*2*math.pi/360 + waypoint.chi_d = temp + waypoint.chi_valid = False + waypoint.Va_d = float(velocity) + waypoint.clear_wp_list = False + waypoint.set_current = False + self.createWaypoint(waypoint) + self.currentWaypoint = waypoint + + + def handleDeleteWaypoint(self): + if not self.waypoints or self.currentWaypoint not in self.waypoints: + print("Can't remove, waypoint not selected or no more waypoints") + return + self.waypoints.remove(self.currentWaypoint) + self.newWaypointList.clear() + for waypoint in self.waypoints: + self.newWaypointList.addItem(self.waypointToString(waypoint)) + + + + def publishedWaypointCallback(self,waypoint): + if waypoint.clear_wp_list == True: + self.currentWaypoints = [] + self.currentWaypointList.clear() + else: + self.currentWaypointList.addItem(self.waypointToString(waypoint)) + + + def handleSendWaypoints(self): + first = True + for waypoint in self.waypoints: + if first: + waypoint.set_current = True + first = False + self.publisher.publish(waypoint) + + + def shutdown_plugin(self): + # TODO unregister all publishers here + pass + + def save_settings(self, plugin_settings, instance_settings): + # TODO save intrinsic configuration, usually using: + # instance_settings.set_value(k, v) + pass + + def restore_settings(self, plugin_settings, instance_settings): + # TODO restore intrinsic configuration, usually using: + # v = instance_settings.value(k) + pass + + #def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget title bar + # Usually used to open a modal configuration dialog \ No newline at end of file