Commit d0db1f13 authored by Valentin Platzgummer's avatar Valentin Platzgummer

Waypoint managers working, not fully testted. Smart RTL optimized,...

Waypoint managers working, not fully testted. Smart RTL optimized, WimaDataContainer renamed to WimaBridge, WimaBridge Optimized.
parent 46978c3e
......@@ -443,9 +443,11 @@ HEADERS += \
src/Wima/WaypointManager/GenericWaypointManager.h \
src/Wima/Geometry/WimaPolygonArray.h \
src/Wima/Snake/snaketile.h \
src/Wima/WaypointManager/RTLManager.h \
src/Wima/WaypointManager/Settings.h \
src/Wima/WaypointManager/Slicer.h \
src/Wima/WaypointManager/Utils.h \
src/Wima/WimaBridge.h \
src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \
src/api/QGCSettings.h \
......@@ -456,7 +458,6 @@ HEADERS += \
src/Wima/Geometry/WimaServiceArea.h \
src/Wima/Geometry/WimaTrackerPolyline.h \
src/Wima/WimaController.h \
src/Wima/WimaDataContainer.h \
src/Wima/WimaPlaner.h \
src/Wima/Geometry/WimaMeasurementArea.h \
src/Wima/Geometry/WimaCorridor.h \
......@@ -502,9 +503,11 @@ SOURCES += \
src/Wima/WaypointManager/AreaInterface.cpp \
src/Wima/WaypointManager/DefaultManager.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \
src/Wima/WaypointManager/RTLManager.cpp \
src/Wima/WaypointManager/Settings.cpp \
src/Wima/WaypointManager/Slicer.cpp \
src/Wima/WaypointManager/Utils.cpp \
src/Wima/WimaBridge.cc \
src/comm/ros_bridge/include/ComPrivateInclude.cpp \
src/comm/ros_bridge/include/MessageTag.cpp \
src/comm/ros_bridge/include/TopicPublisher.cpp \
......@@ -520,7 +523,6 @@ SOURCES += \
src/Wima/Geometry/WimaServiceArea.cc \
src/Wima/Geometry/WimaTrackerPolyline.cc \
src/Wima/WimaController.cc \
src/Wima/WimaDataContainer.cc \
src/Wima/WimaPlaner.cc \
src/Wima/Geometry/WimaMeasurementArea.cc \
src/Wima/Geometry/WimaCorridor.cc \
......
......@@ -118,15 +118,22 @@ QGCView {
WimaController {
id: wimaController
Component.onCompleted: {
wimaController.dataContainer = Qt.binding(function() { return dataContainerPointer })
wBridge.wimaController = Qt.binding(function() { return wimaController.thisPointer() })
}
onReturnUserRequestConfirmRequired: {
_guidedController.confirmAction(_guidedController.actionUploadAndExecute)
onForceUploadConfirm: {
_guidedController.confirmAction(
_guidedController.actionForceUpload)
}
onReturnBatteryLowConfirmRequired: {
_guidedController.confirmAction(_guidedController.actionReturnBatteryLow)
onSmartRTLRequestConfirm: {
_guidedController.confirmAction(
_guidedController.actionSmartRTLRequestConfirm)
}
onSmartRTLPathConfirm: {
_guidedController.confirmAction(
_guidedController.actionSmartRTLPathConfirm)
}
}
......@@ -765,14 +772,6 @@ QGCView {
altitudeSlider: _altitudeSlider
z: _flightVideoPipControl.z + 1
property bool uploadOverrideRequired: wimaController.uploadOverrideRequired
onUploadOverrideRequiredChanged: {
if (uploadOverrideRequired) {
confirmAction(actionOverrideUpload)
}
}
onShowStartMissionChanged: {
if (showStartMission) {
confirmAction(actionStartMission)
......
......@@ -354,7 +354,7 @@ Item {
text: qsTr("Upload")
onClicked: {
if (!planMasterController.offline) {
wimaController.uploadToVehicle()
wimaController.upload()
}
}
Layout.fillWidth: true
......
......@@ -52,9 +52,9 @@ Item {
readonly property string setWaypointTitle: qsTr("Set Waypoint")
readonly property string gotoTitle: qsTr("Goto Location")
readonly property string vtolTransitionTitle: qsTr("VTOL Transition")
readonly property string overrideUploadTitle: qsTr("Override Lock")
readonly property string uploadAndExecuteTitle: qsTr("Upload and Execute")
readonly property string returnBatteryLowTitle: qsTr("Battery Low")
readonly property string forceUploadTitle: qsTr("Force Upload")
readonly property string smartRTLPathConfirmTitle: qsTr("Upload and Execute")
readonly property string smartRTLRequestConfirmTitle: qsTr("Smart RTL requested")
readonly property string armMessage: qsTr("Arm the vehicle.")
readonly property string disarmMessage: qsTr("Disarm the vehicle")
......@@ -74,9 +74,9 @@ Item {
readonly property string mvPauseMessage: qsTr("Pause all vehicles at their current position.")
readonly property string vtolTransitionFwdMessage: qsTr("Transition VTOL to fixed wing flight.")
readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.")
readonly property string overrideUploadMessage: qsTr("Vehicle is not inside service area. Upload nevertheless?")
readonly property string uploadAndExecuteMessage: qsTr("Upload and execute the displayed return path?")
readonly property string returnBatteryLowMessage: qsTr("Upload and execute the displayed return path?")
readonly property string forceUploadMessage: qsTr("Vehicle outside save area. Force upload?")
readonly property string smartRTLPathConfirmMessage: qsTr("Upload and execute the displayed return path?")
readonly property string smartRTLRequestConfirmMessage: qsTr("Do you want to initiate a smart RTL?")
readonly property int actionRTL: 1
......@@ -100,9 +100,9 @@ Item {
readonly property int actionMVStartMission: 19
readonly property int actionVtolTransitionToFwdFlight: 20
readonly property int actionVtolTransitionToMRFlight: 21
readonly property int actionOverrideUpload: 22
readonly property int actionUploadAndExecute: 24
readonly property int actionReturnBatteryLow: 25 // very similar to actionUploadAndExecute, is triggered on low battery
readonly property int actionForceUpload: 22
readonly property int actionSmartRTLPathConfirm: 24
readonly property int actionSmartRTLRequestConfirm: 25 // very similar to actionSmartRTLPathConfirm, is triggered on low battery
property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying
property bool showArm: _guidedActionsEnabled && !_vehicleArmed
......@@ -343,19 +343,19 @@ Item {
confirmDialog.message = vtolTransitionMRMessage
confirmDialog.hideTrigger = true
break
case actionOverrideUpload:
confirmDialog.title = overrideUploadTitle
confirmDialog.message = overrideUploadMessage
case actionForceUpload:
confirmDialog.title = forceUploadTitle
confirmDialog.message = forceUploadMessage
confirmDialog.hideTrigger = true
break
case actionUploadAndExecute:
confirmDialog.title = uploadAndExecuteTitle
confirmDialog.message = uploadAndExecuteMessage
case actionSmartRTLPathConfirm:
confirmDialog.title = smartRTLPathConfirmTitle
confirmDialog.message = smartRTLPathConfirmMessage
confirmDialog.hideTrigger = true
break
case actionReturnBatteryLow:
confirmDialog.title = returnBatteryLowTitle
confirmDialog.message = returnBatteryLowMessage
case actionSmartRTLRequestConfirm:
confirmDialog.title = smartRTLRequestConfirmTitle
confirmDialog.message = smartRTLRequestConfirmMessage
confirmDialog.hideTrigger = true
break
default:
......@@ -433,13 +433,15 @@ Item {
case actionVtolTransitionToMRFlight:
_activeVehicle.vtolInFwdFlight = false
break
case actionOverrideUpload:
wimaController.forceUploadToVehicle()
case actionForceUpload:
wimaController.forceUpload()
break
case actionUploadAndExecute:
case actionReturnBatteryLow:
case actionSmartRTLPathConfirm:
wimaController.executeSmartRTL()
break
case actionSmartRTLRequestConfirm:
wimaController.initSmartRTL()
break
default:
console.warn(qsTr("Internal error: unknown actionCode"), actionCode)
break
......
......@@ -68,7 +68,7 @@
#include "CoordinateVector.h"
#include "PlanMasterController.h"
#include "Wima/WimaController.h"
#include "Wima/WimaDataContainer.h"
#include "Wima/WimaBridge.h"
#include "Wima/WimaPlaner.h"
#include "VideoManager.h"
#include "VideoSurface.h"
......@@ -468,7 +468,7 @@ void QGCApplication::_initCommon(void)
// Wima
qmlRegisterType<WimaController> ("Wima", 1, 0, "WimaController");
qmlRegisterType<WimaPlaner> ("Wima", 1, 0, "WimaPlaner");
qmlRegisterType<WimaDataContainer> ("Wima", 1, 0, "WimaDataContainer");
qmlRegisterType<WimaBridge> ("Wima", 1, 0, "WimaBridge");
// Register Qml Singletons
......
......@@ -108,6 +108,11 @@ bool WaypointManager::DefaultManager::_calcShortestPath(
bool WaypointManager::DefaultManager::_worker()
{
// Precondition:
// _waypoints must contain valid coordinates.
// Slicer must be called befor invoking this function.
// E.g. Slicer::reset(_waypoints, _currentWaypoints);
using namespace WaypointManager::Utils;
if (_waypoints.count() < 1 || !_settings->valid()) {
......@@ -131,13 +136,6 @@ bool WaypointManager::DefaultManager::_worker()
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView());
// Precondition.
if (!_settings->homePosition().isValid()){
qWarning("WaypointManager::DefaultManager::next(): home position invalid.");
Q_ASSERT(false);
return false;
}
// Calculate path from home to first waypoint.
QVector<QGeoCoordinate> arrivalPath;
if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
......@@ -226,9 +224,7 @@ bool WaypointManager::DefaultManager::_worker()
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_currentWaypoints.last());
speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed());
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
......@@ -246,38 +242,37 @@ bool WaypointManager::DefaultManager::_worker()
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND;
if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) {
landItem->setCommand(landCmd);
} else {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): Land command not supported!");
return false;
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
item->altitude()->setRawValue(_settings->altitude());
}
// Update list _currentMissionItems.
updateHirarchy(_currentMissionItems);
updateSequenceNumbers(_currentMissionItems);
// Prepend arrival path to slice.
for ( long i = arrivalPath.size()-1; i >=0; --i )
_currentWaypoints.push_front(arrivalPath[i]);
_currentWaypoints.push_front(_settings->homePosition());
// Append return path to slice.
for ( auto c : returnPath )
_currentWaypoints.push_back(c);
_currentWaypoints.push_back(_settings->homePosition());
// Create variant list.
_currentWaypointsVariant.clear();
for ( auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
item->altitude()->setRawValue(_settings->altitude());
}
return true;
}
......@@ -19,13 +19,13 @@ public:
GenericWaypointManager(SettingsType &settings);
// Waypoint editing.
void setWaypoints(const WaypointList &waypoints);
void push_back (const WaypointType &wp);
void push_front (const WaypointType &wp);
virtual void clear ();
void insert (std::size_t i, const WaypointType &wp);
std::size_t size () const;
WaypointType &at (std::size_t i);
virtual void setWaypoints(const WaypointList &waypoints);
virtual void push_back (const WaypointType &wp);
virtual void push_front (const WaypointType &wp);
virtual void clear ();
virtual void insert (std::size_t i, const WaypointType &wp);
virtual std::size_t size () const;
virtual void remove (std::size_t i);
const WaypointList &waypoints() const;
......@@ -141,12 +141,12 @@ template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
WaypointType & GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::at(std::size_t i)
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::remove(std::size_t i)
{
return _waypoints.at(i);
return _waypoints.remove(i);
}
template<class WaypointType,
......
#include "RTLManager.h"
#include "Wima/Geometry/GeoUtilities.h"
#include "Wima/Geometry/PolygonCalculus.h"
#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"
WaypointManager::RTLManager::RTLManager(Settings &settings,
AreaInterface &interface)
: ManagerBase(settings)
, _areaInterface(&interface)
{
}
void WaypointManager::RTLManager::setWaypoints(const QVector<QGeoCoordinate> &waypoints)
{
(void)waypoints;
return;
}
void WaypointManager::RTLManager::push_back(const QGeoCoordinate &wp)
{
(void)wp;
return;
}
void WaypointManager::RTLManager::push_front(const QGeoCoordinate &wp)
{
(void)wp;
return;
}
void WaypointManager::RTLManager::clear()
{
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
void WaypointManager::RTLManager::insert(std::size_t i, const QGeoCoordinate &wp)
{
(void)i;
(void)wp;
return;
}
std::size_t WaypointManager::RTLManager::size() const
{
return 0;
}
void WaypointManager::RTLManager::remove(std::size_t i)
{
(void)i;
}
bool WaypointManager::RTLManager::update()
{
this->clear();
return _worker();
}
bool WaypointManager::RTLManager::next()
{
return true;
}
bool WaypointManager::RTLManager::previous()
{
return true;
}
bool WaypointManager::RTLManager::reset()
{
return true;
}
QString WaypointManager::RTLManager::checkPrecondition()
{
Vehicle *managerVehicle = _settings->masterController()->managerVehicle();
if (!managerVehicle->flying()) {
return QString("Vehicle is not flying. Smart RTL not available.");
}
if (!managerVehicle->isOfflineEditingVehicle()) {
return QString("Not connected to any vehicle. Smart RTL not available.");
}
if (!_areaInterface->joinedArea()->containsCoordinate(managerVehicle->coordinate())) {
return QString("Vehicle not inside save area. Smart RTL not available.");
}
return QString();
}
bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
QmlObjectListModel &list,
bool doUpdate)
{
using namespace WaypointManager::Utils;
if ( !insertMissionItem(c,
index /*insertion index*/,
list,
_settings->vehicle(),
_settings->isFlyView(),
&list /*parent*/,
doUpdate /*do update*/) )
{
qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
return false;
}
return true;
}
bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate)
{
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}
bool WaypointManager::RTLManager::_calcShortestPath(
const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF joinedArea2D;
toCartesianList(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start, joinedArea2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2DOut;
bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
toGeoList(path2DOut, /*origin*/ start, path);
return retVal;
}
bool WaypointManager::RTLManager::_worker()
{
// Precondition: settings must be valid.
using namespace WaypointManager::Utils;
if (!_settings->valid()) {
return false;
}
initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView());
// Calculate path from vehicle to home position.
QVector<QGeoCoordinate> returnPath;
auto vehicleCoordinate = _settings->masterController()->managerVehicle()->coordinate();
if ( !_calcShortestPath(vehicleCoordinate, _settings->homePosition(), returnPath) ) {
qWarning("WaypointManager::RTLManager::next(): Not able to calc path from vehicle to home position.");
return false;
}
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem* settingsItem = _currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::RTLManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(_settings->homePosition());
// Create change speed item.
_insertMissionItem(_settings->homePosition(),
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
if (speedItem == nullptr) {
qWarning("WaypointManager::RTLManager::next(): nullptr.");
Q_ASSERT(speedItem != nullptr);
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Set land command for last mission item.
int index = _currentMissionItems.count();
_insertMissionItem(_settings->homePosition(),
index /*insertion index*/,
false /*do update*/);
SimpleMissionItem *landItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (landItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::RTLManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
item->altitude()->setRawValue(_settings->altitude());
}
// Update list _currentMissionItems.
updateHirarchy(_currentMissionItems);
updateSequenceNumbers(_currentMissionItems);
// Append return path to _currentWaypoints.
for ( auto c : returnPath )
_currentWaypoints.push_back(c);
// Create variant list.
_currentWaypointsVariant.clear();
for ( auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
return true;
}
#pragma once
#include "GenericWaypointManager.h"
#include "Settings.h"
#include "AreaInterface.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate,
QVector,
QmlObjectListModel,
Settings>
ManagerBase;
//!
//! \brief The RTLManager class is used to manage the return to launch of the vehicle.
//!
//! @note The \class QmlObjectList returned by \fn missionItems doesn't contain a MissionSettingsItem.
//! This list is supposed to be used for display purposes only.
class RTLManager : public ManagerBase
{
public:
RTLManager() = delete;
RTLManager(Settings &settings,
AreaInterface &interface);
//!
//! \brief Does nothing.
//!
virtual void setWaypoints(const QVector<QGeoCoordinate> &waypoints) override;
//!
//! \brief Does nothing.
//!
virtual void push_back (const QGeoCoordinate &wp) override;
//!
//! \brief Does nothing.
//!
virtual void push_front (const QGeoCoordinate &wp) override;
//!
//! \brief Clears the waypoint manager.
virtual void clear () override;
//!
//! \brief Does nothing.
//!
virtual void insert (std::size_t i, const QGeoCoordinate &wp) override;
//!
//! \brief Returns zero.
//! \return Returns zero.
virtual std::size_t size () const override;
//!
//! \brief Returns an empty coordinate.
virtual void remove (std::size_t i) override;
//!
//! \brief Updates the waypoint manager.
//!
//! After updateing a call to currentMissionItems or currentWaypoints returns
//! a path from the vehicles current position to the home position.
//!
//! \return Returns true on success, false either.
//!
virtual bool update() override;
//!
//! \brief Does nothing.
//! \return Returns true.
//!
virtual bool next() override;
//!
//! \brief Does nothing.
//! \return Returns true.
//!
virtual bool previous() override;
//!
//! \brief Does nothing.
//! \return Returns true.
//!
virtual bool reset() override;
QString checkPrecondition();
protected:
bool _insertMissionItem(const QGeoCoordinate &c,
size_t index,
QmlObjectListModel &list,
bool doUpdate);
bool _insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate);
bool _calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
AreaInterface *_areaInterface;
private:
bool _worker();
};
} // namespace WaypointManager
#include "Slicer.h"
Slicer::Slicer():
_idxValid(false)
, _atEnd(false)
Slicer::Slicer()
: _idxStart(0)
, _idxEnd(0)
, _idxNext(0)
, _idxPrevious(0)
, _overlap(0)
, _N(0)
{}
void Slicer::setOverlap(uint32_t overlap)
{
_idxValid = false;
_overlap = overlap;
}
void Slicer::setN(uint32_t N)
{
_idxValid = false;
_N = N > 0 ? N : 1;
}
void Slicer::setStartIndex(int idxStart)
{
_idxValid = false;
_idxStart = idxStart;
}
......@@ -38,28 +39,30 @@ int Slicer::startIndex()
return _idxStart;
}