Skip to content

Q
qgroundcontrol
Commit d02c3c46 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

refactoring WimaController, adding waypoint managers.

parent 8b365d4f
Hide whitespace changes
Inline Side-by-side
Showing with 607 additions and 219 deletions
qgroundcontrol.pro
View file @ d02c3c46
......@@ -437,7 +437,9 @@ HEADERS += \
src/Wima/QtROSTypeFactory.h \
src/Wima/SnakeTiles.h \
src/Wima/SnakeTilesLocal.h \
src/Wima/WimaControllerDetail.h \
src/Wima/SnakeWorker.h \
src/Wima/WaypointManager/GenericPathSlicer.h \
src/Wima/WaypointManager/GenericWaypointManager.h \
src/Wima/WimaPolygonArray.h \
src/Wima/snaketile.h \
src/api/QGCCorePlugin.h \
......@@ -493,12 +495,14 @@ SOURCES += \
src/Wima/GeoPoint3D.cpp \
src/Wima/PolygonArray.cc \
src/Wima/QNemoProgress.cc \
src/Wima/SnakeWorker.cc \
src/Wima/WaypointManager/GenericPathSlicer.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \
src/comm/ros_bridge/include/ComPrivateInclude.cpp \
src/comm/ros_bridge/include/MessageTag.cpp \
src/comm/ros_bridge/include/TopicPublisher.cpp \
src/comm/ros_bridge/include/TopicSubscriber.cpp \
src/comm/ros_bridge/src/CasePacker.cpp \
src/Wima/WimaControllerDetail.cc \
src/Wima/snaketile.cpp \
src/api/QGCCorePlugin.cc \
src/api/QGCOptions.cc \
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ d02c3c46
......@@ -244,9 +244,12 @@ FlightMap {
// Add Snake tiles center points to the map
MapItemView {
property bool _lengthMatching: wimaController.snakeTileCenterPoints.length
=== wimaController.snakeProgress.length
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
&& _lengthMatching
model: _enable ? wimaController.snakeTileCenterPoints : 0
......
src/MissionManager/MissionController.cc
View file @ d02c3c46
......@@ -378,7 +378,7 @@ int MissionController::_nextSequenceNumber(void)
}
}
int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
int MissionController::insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i)
{
int sequenceNumber = _nextSequenceNumber();
SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
......@@ -410,6 +410,58 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
return newItem->sequenceNumber();
}
int MissionController::insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int idx)
{
MAV_CMD mavCmd = MAV_CMD_NAV_WAYPOINT;
if ( _visualItems->count() == 1
&& ( _controllerVehicle->fixedWing()
|| _controllerVehicle->vtol()
|| _controllerVehicle->multiRotor()) )
{
mavCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
if ( !_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(mavCmd)) {
mavCmd = MAV_CMD_NAV_WAYPOINT;
}
}
int sequenceNumber = _nextSequenceNumber();
int size = coordinates.size();
SimpleMissionItem *newItem = nullptr;
bool firstItem = true;
double prevAltitude = 0.0;
int prevAltitudeMode = 0;
for (int i = 0; i < size; ++i) {
newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
newItem->setSequenceNumber(sequenceNumber);
newItem->setCoordinate(coordinates[i]);
newItem->setCommand(mavCmd);
_initSimpleItem(newItem);
if (!firstItem || ( newItem->specifiesAltitude()
&& _findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) )
{
newItem->altitude()->setRawValue(prevAltitude);
newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
}
newItem->setMissionFlightStatus(_missionFlightStatus);
_visualItems->insert(idx++, newItem);
sequenceNumber = newItem->lastSequenceNumber() + 1;
mavCmd = MAV_CMD_NAV_WAYPOINT;
firstItem = false;
}
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
_updateTimer.start(UPDATE_TIMEOUT);
if ( newItem == nullptr)
return -1;
return newItem->sequenceNumber();
}
int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, int i)
{
int sequenceNumber = _nextSequenceNumber();
......@@ -1674,7 +1726,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo
}
/// @param clickCoordinate The location of the user click when inserting a new item
void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate)
void MissionController::_recalcAllWithClickCoordinate(const QGeoCoordinate& clickCoordinate)
{
if (!_flyView) {
_setPlannedHomePositionFromFirstCoordinate(clickCoordinate);
......@@ -1770,37 +1822,61 @@ void MissionController::_deinitAllVisualItems(void)
void MissionController::_initVisualItem(VisualMissionItem* visualItem)
{
setDirty(false);
connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged, this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged, this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::terrainAltitudeChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);
if (visualItem->isSimpleItem()) {
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
if (simpleItem) {
connect(&simpleItem->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
_initSimpleItem(simpleItem);
} else {
qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
}
} else {
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
if (complexItem) {
connect(complexItem, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(complexItem, &ComplexMissionItem::greatestDistanceToChanged, this, &MissionController::_recalcMissionFlightStatus);
_initComplexItem(complexItem);
} else {
qWarning() << "ComplexMissionItem not found";
}
}
}
void MissionController::_initVisualItemCommon(VisualMissionItem *visualItem)
{
setDirty(false);
connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,
this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,
this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,
this, &MissionController::_recalcWaypointLines);
connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,
this, &MissionController::_recalcMissionFlightStatus);
connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,
this, &MissionController::_recalcSequence);
}
void MissionController::_initSimpleItem(SimpleMissionItem *item)
{
_initVisualItemCommon(item);
// We need to track commandChanged on simple item since recalc has special handling for takeoff command
connect(&item->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
}
void MissionController::_initComplexItem(ComplexMissionItem *item)
{
_initVisualItemCommon(item);
connect(item, &ComplexMissionItem::complexDistanceChanged, this, &MissionController::_recalcMissionFlightStatus);
connect(item, &ComplexMissionItem::greatestDistanceToChanged, this, &MissionController::_recalcMissionFlightStatus);
}
void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
{
// Disconnect all signals
......
src/MissionManager/MissionController.h
View file @ d02c3c46
......@@ -109,7 +109,12 @@ public:
/// Add a new simple mission item to the list
/// @param i: index to insert at
/// @return Sequence number for new item
Q_INVOKABLE int insertSimpleMissionItem(QGeoCoordinate coordinate, int i);
Q_INVOKABLE int insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i);
/// Add a list of new simple mission item to the list.
/// @param i: index to insert at
/// @return Sequence number of last item.
Q_INVOKABLE int insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int i);
/// Add a new simple mission item to the list
/// @param i: index to insert at
......@@ -289,10 +294,13 @@ private:
void _init(void);
void _recalcSequence(void);
void _recalcChildItems(void);
void _recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate);
void _recalcAllWithClickCoordinate(const QGeoCoordinate &clickCoordinate);
void _initAllVisualItems(void);
void _deinitAllVisualItems(void);
void _initVisualItem(VisualMissionItem* item);
void _initVisualItemCommon(VisualMissionItem* visualItem); // Don't call this function, it is used by _initSimpleItem and _initComplexItem.
void _initSimpleItem(SimpleMissionItem* item);
void _initComplexItem(ComplexMissionItem* item);
void _deinitVisualItem(VisualMissionItem* item);
void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
......
src/Snake/snake_geometry.cpp
View file @ d02c3c46
......@@ -274,7 +274,8 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, dou
bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy);
polygonOffset = result[0];
if (result.size() > 0)
polygonOffset = result[0];
}
......
src/Wima/QNemoProgress.cc
View file @ d02c3c46
#include "QNemoProgress.h"
QNemoProgress::QNemoProgress(QObject *parent) :
ProgressBase()
, QObject(parent)
{
}
QNemoProgress::QNemoProgress(const QNemoProgress &other, QObject *parent):
ProgressBase(other)
, QObject(parent)
{
}
QNemoProgress *QNemoProgress::Clone() const {
return new QNemoProgress(*this, this->parent());
}
QVector<int> &QNemoProgress::progress() {
emit QNemoProgress::progressChanged();
return _progress;
}
const QVector<int> &QNemoProgress::progress() const {
return _progress;
}
src/Wima/QNemoProgress.h
View file @ d02c3c46
......@@ -5,22 +5,6 @@
#include "ros_bridge/include/GenericMessages.h"
namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
typedef NemoMsgs::GenericProgress<int, QVector> ProgressBase;
class QNemoProgress : public ProgressBase, public QObject {
public:
QNemoProgress(QObject *parent = nullptr);
QNemoProgress(const QNemoProgress &other, QObject *parent = nullptr);
virtual QNemoProgress *Clone() const override;
virtual const QVector<int> &progress(void) const override;
virtual QVector<int> &progress(void) override;
signals:
void progressChanged();
};
typedef NemoMsgs::GenericProgress<int, QVector> QNemoProgress;
src/Wima/WimaControllerDetail.cc → src/Wima/SnakeWorker.cc
View file @ d02c3c46
#include "WimaControllerDetail.h"
#include "SnakeWorker.h"
#include <QGeoCoordinate>
......@@ -6,7 +6,10 @@ SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
, _lineDistance (3) // meter
, _minTransectLength (2) // meter
{
}
SnakeWorker::~SnakeWorker()
{
}
void SnakeWorker::setScenario(const Scenario &scenario)
......@@ -80,6 +83,4 @@ void SnakeWorker::run()
}
}
emit resultReady();
}
src/Wima/WimaControllerDetail.h → src/Wima/SnakeWorker.h
View file @ d02c3c46
......@@ -28,6 +28,7 @@ class SnakeWorker : public QThread{
public:
SnakeWorker(QObject *parent = nullptr);
~SnakeWorker() override;
void setScenario (const Scenario &scenario);
void setProgress (const QVector<int> &progress);
......@@ -36,9 +37,6 @@ public:
const WorkerResult_t &getResult() const;
signals:
void resultReady();
protected:
void run() override;
......
src/Wima/WaypointManager/GenericPathSlicer.h 0 → 100644
View file @ d02c3c46
#pragma once
#include <assert.h>
#include <iostream>
//! @brief Base class for all waypoint managers.
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
class GenericPathSlicer
{
public:
typedef Container<ElementType> ContainerType;
GenericPathSlicer();
const ContainerType &path () const;
// Waypoint editing.
void setPath (const ContainerType &path);
void push_back (const ElementType &wp);
void push_front (const ElementType &wp);
void clear ();
void insert (int i, const ElementType &wp);
uint32_t size();
ElementType &at (unsigned int i);
// Slicing.
void slice (ContainerType &slice);
void next (ContainerType &slice);
void previous (ContainerType &slice);
void reset (ContainerType &slice);
// Slicing parameters.
//! @brief Sets the overlap.
//!
//! @param overlap The number of overlapping vertices
//! between on and the next slice.
void setOverlap (uint32_t overlap);
//! @brief Sets the number of vertices per slice.
//!
//! @param N The number of vertices per slice.
void setN (std::uint32_t N);
//! @brief Sets the start index.
//!
//! @param idxStart The start index.
void setStartIndex (int idxStart);
//! @return Returns the overlap.
uint32_t overlap ();
//! @return Returns the number of vertices per slice N.
uint32_t N ();
//! @return Returns the start index.
int startIndex ();
//! @return Returns the end index.
int endIndex ();
//! @return Returns the start index of the next slice.
int nextIndex ();
private:
void _updateIdx();
ContainerType _path;
long _idxStart;
long _idxEnd;
long _idxNext;
long _idxPrevious;
uint32_t _overlap;
uint32_t _N;
bool _idxValid;
bool _atEnd;
};
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
GenericPathSlicer<ElementType, Container>::GenericPathSlicer():
_idxValid(false)
, _atEnd(false)
{}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
const Container<ElementType> &GenericPathSlicer<ElementType, Container>::path() const {
return _path;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setPath(const ContainerType &waypoints) {
_idxValid = false;
_path = waypoints;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::push_back(const ElementType &wp) {
_idxValid = false;
_path.push_back(wp);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::push_front(const ElementType &wp) {
_idxValid = false;
_path.push_front(wp);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::clear(){
_idxValid = false;
_path.clear();
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::insert(int i, const ElementType &wp){
_idxValid = false;
_path.insert(i, wp);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
std::uint32_t GenericPathSlicer<ElementType, Container>::size()
{
return std::uint32_t(_path.size());
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
ElementType &GenericPathSlicer<ElementType, Container>::at(unsigned int i)
{
return _path.at(i);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setOverlap(std::uint32_t overlap)
{
_idxValid = false;
_overlap = overlap;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setN(uint32_t N)
{
_idxValid = false;
_N = N > 0 ? N : 1;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setStartIndex(int idxStart)
{
_idxValid = false;
_idxStart = idxStart;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
uint32_t GenericPathSlicer<ElementType, Container>::overlap()
{
return _overlap;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
uint32_t GenericPathSlicer<ElementType, Container>::N()
{
return _N;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<ElementType, Container>::startIndex()
{
if (!_idxValid)
_updateIdx();
return _idxStart;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<ElementType, Container>::endIndex()
{
if (!_idxValid)
_updateIdx();
return _idxEnd;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<ElementType, Container>::nextIndex()
{
if (!_idxValid)
_updateIdx();
return _idxNext;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::_updateIdx()
{
_idxValid = true;
_atEnd = false;
std::uint64_t size = _path.size();
if ( _idxStart >= size-1 ) {
_idxStart = size-1;
_idxEnd = _idxStart;
_idxNext = _idxStart;
_atEnd = true;
return;
}
_idxStart < 0 ? 0 : _idxStart;
_idxEnd = _idxStart + _N - 1;
_idxEnd = _idxEnd < size ? _idxEnd : size-1;
_idxNext = _idxEnd + 1 - _overlap;
_idxNext = _idxNext < 0 ? 0 : _idxNext;
_idxNext = _idxNext < size ? _idxNext : size-1;
_idxPrevious = _idxStart - 1 + _overlap;
_idxPrevious = _idxPrevious < 0 ? 0 : _idxPrevious;
_idxPrevious = _idxPrevious < size ? _idxPrevious : size-1;
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::slice(Container<ElementType> &c){
if ( !_idxValid)
_updateIdx();
(void)c;
assert(false); //ToDo
// extract waypoints
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::next(Container<ElementType> &c){
setStartIndex(_idxNext);
slice(c);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::previous(Container<ElementType> &c){
setStartIndex(_idxPrevious);
slice(c);
}
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::reset(Container<ElementType> &c){
setStartIndex(0);
slice(c);
}
src/Wima/WaypointManager/GenericWaypointManager.cpp 0 → 100644
View file @ d02c3c46
#include "GenericWaypointManager.h"
GenericWaypointManager::GenericWaypointManager()
{
}
src/Wima/WaypointManager/GenericWaypointManager.h 0 → 100644
View file @ d02c3c46
#pragma once
class GenericWaypointManager
{
public:
GenericWaypointManager();
};
src/Wima/WimaController.cc
View file @ d02c3c46
......@@ -17,7 +17,7 @@
#include <QScopedPointer>
const char* WimaController::wimaFileExtension = "wima";
// const char* WimaController::wimaFileExtension = "wima";
const char* WimaController::areaItemsName = "AreaItems";
const char* WimaController::missionItemsName = "MissionItems";
const char* WimaController::settingsGroup = "WimaController";
......@@ -101,29 +101,32 @@ WimaController::WimaController(QObject *parent)
_enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());
// Snake Worker Thread.
connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults);
connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
// Start, stop RosBridge.
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
_startStopRosBridge();
}
QStringList WimaController::loadNameFilters() const
{
QStringList filters;
//QStringList WimaController::loadNameFilters() const
//{
// QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
tr("All Files (*.*)");
return filters;
}
// filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
// tr("All Files (*.*)");
// return filters;
//}
QStringList WimaController::saveNameFilters() const
{
QStringList filters;
//QStringList WimaController::saveNameFilters() const
//{
// QStringList filters;
filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
return filters;
}
// filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
// return filters;
//}
bool WimaController::uploadOverrideRequired() const
{
......@@ -331,37 +334,32 @@ void WimaController::removeVehicleTrajectoryHistory()
managerVehicle->trajectoryPoints()->clear();
}
void WimaController::saveToCurrent()
{
//void WimaController::saveToFile(const QString& filename)
//{
// QString file = filename;
//}
}
//bool WimaController::loadFromCurrent()
//{
// return true;
//}
void WimaController::saveToFile(const QString& filename)
{
QString file = filename;
}
//bool WimaController::loadFromFile(const QString &filename)
//{
// QString file = filename;
// return true;
//}
bool WimaController::loadFromCurrent()
{
return true;
}
bool WimaController::loadFromFile(const QString &filename)
{
QString file = filename;
return true;
}
//QJsonDocument WimaController::saveToJson(FileType fileType)
//{
// if(fileType)
// {
QJsonDocument WimaController::saveToJson(FileType fileType)
{
if(fileType)
{
}
return QJsonDocument();
}
// }
// return QJsonDocument();
//}
bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path)
{
......@@ -1002,31 +1000,21 @@ void WimaController::_eventTimerHandler()
_checkBatteryLevel();
// Snake flight plan update necessary?
if ( snakeEventLoopTicker.ready() ) {
if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
_snakeWorker.setScenario(_scenario);
_snakeWorker.setProgress(_snakeProgress.progress());
_snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
_snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
_setSnakeCalcInProgress(true);
_snakeWorker.start();
emit snakeProgressChanged();
}
}
// if ( snakeEventLoopTicker.ready() ) {
// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
// }
// }
if (rosBridgeTicker.ready()) {
// using namespace ros_bridge;
_pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
_pRosBridge->publish(_snakeOrigin, "/snake/origin");
using namespace std::placeholders;
auto callBack = std::bind(ProgressFromJson,
std::ref(*(_pRosBridge->casePacker())),
auto callBack = std::bind(&WimaController::_progressFromJson,
this,
_1,
std::ref(_snakeProgress));
std::ref(_nemoProgress));
_pRosBridge->subscribe("/nemo/progress", callBack);
}
}
......@@ -1372,13 +1360,34 @@ void WimaController::_snakeStoreWorkerResults()
void WimaController::_startStopRosBridge()
{
if (_enableSnake.rawValue().toBool()) {
if ( _enableSnake.rawValue().toBool() ) {
_pRosBridge->start();
} else {
_pRosBridge->stop();
_pRosBridge->reset();
}
}
void WimaController::_initStartSnakeWorker()
{
if ( !_enableSnake.rawValue().toBool() )
return;
// Stop worker thread if running.
if ( _snakeWorker.isRunning() ) {
_snakeWorker.quit();
}
// Initialize _snakeWorker.
_snakeWorker.setScenario(_scenario);
_snakeWorker.setProgress(_nemoProgress.progress());
_snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
_snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
_setSnakeCalcInProgress(true);
// Start worker thread.
_snakeWorker.start();
}
void WimaController::_loadCurrentMissionItemsFromBuffer()
{
_currentMissionItems.clear();
......@@ -1398,7 +1407,14 @@ void WimaController::_saveCurrentMissionItemsToBuffer()
_missionItemsBuffer.append(_currentMissionItems.removeAt(0));
}
void ProgressFromJson(const ROSBridge::CasePacker &casePacker, JsonDocUPtr pDoc, QNemoProgress &progress)
void WimaController::_progressFromJson(JsonDocUPtr pDoc,
QNemoProgress &progress)
{
casePacker.unpack(pDoc, progress);
int requiredSize = _snakeTilesLocal.polygons().size();
if ( !_pRosBridge->casePacker()->unpack(pDoc, progress)
|| progress.progress().size() != requiredSize ) {
progress.progress().fill(0, requiredSize);
}
emit WimaController::nemoProgressChanged();
}
src/Wima/WimaController.h
View file @ d02c3c46
......@@ -27,7 +27,7 @@
#include "SettingsManager.h"
#include "snake.h"
#include "WimaControllerDetail.h"
#include "SnakeWorker.h"
#include "SnakeTiles.h"
#include "SnakeTilesLocal.h"
#include "GeoPoint3D.h"
......@@ -65,10 +65,10 @@ public:
Q_PROPERTY(PlanMasterController* masterController READ masterController WRITE setMasterController NOTIFY masterControllerChanged)
Q_PROPERTY(MissionController* missionController READ missionController WRITE setMissionController NOTIFY missionControllerChanged)
Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged)
Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged)
Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT)
Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT)
Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT)
// Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged)
// Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT)
// Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT)
// Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT)
Q_PROPERTY(WimaDataContainer* dataContainer READ dataContainer WRITE setDataContainer NOTIFY dataContainerChanged)
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged)
Q_PROPERTY(QmlObjectListModel* currentMissionItems READ currentMissionItems NOTIFY currentMissionItemsChanged)
......@@ -100,7 +100,7 @@ public:
Q_PROPERTY(Fact* snakeMinTransectLength READ snakeMinTransectLength CONSTANT)
Q_PROPERTY(QmlObjectListModel* snakeTiles READ snakeTiles NOTIFY snakeTilesChanged)
Q_PROPERTY(QVariantList snakeTileCenterPoints READ snakeTileCenterPoints NOTIFY snakeTileCenterPointsChanged)
Q_PROPERTY(QVector<int> *snakeProgress READ snakeProgress NOTIFY snakeProgressChanged)
Q_PROPERTY(QVector<int> nemoProgress READ nemoProgress NOTIFY nemoProgressChanged)
......@@ -108,10 +108,10 @@ public:
PlanMasterController* masterController (void) { return _masterController; }
MissionController* missionController (void) { return _missionController; }
QmlObjectListModel* visualItems (void) { return &_visualItems; }
QString currentFile (void) const { return _currentFile; }
QStringList loadNameFilters (void) const;
QStringList saveNameFilters (void) const;
QString fileExtension (void) const { return wimaFileExtension; }
// QString currentFile (void) const { return _currentFile; }
// QStringList loadNameFilters (void) const;
// QStringList saveNameFilters (void) const;
// QString fileExtension (void) const { return wimaFileExtension; }
QGCMapPolygon joinedArea (void) const;
WimaDataContainer* dataContainer (void) { return _container; }
QmlObjectListModel* missionItems (void) { return &_missionItems; }
......@@ -137,7 +137,7 @@ public:
Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;}
QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();}
QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;}
QVector<int> *snakeProgress (void) { return &_snakeProgress.progress();}
QVector<int> nemoProgress (void) { return _nemoProgress.progress();}
bool uploadOverrideRequired (void) const;
double phaseDistance (void) const;
......@@ -168,14 +168,14 @@ public:
Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE void saveToCurrent ();
Q_INVOKABLE void saveToFile (const QString& filename);
Q_INVOKABLE bool loadFromCurrent();
Q_INVOKABLE bool loadFromFile (const QString& filename);
// Q_INVOKABLE void saveToCurrent ();
// Q_INVOKABLE void saveToFile (const QString& filename);
// Q_INVOKABLE bool loadFromCurrent();
// Q_INVOKABLE bool loadFromFile (const QString& filename);
// static Members
static const char* wimaFileExtension;
// static const char* wimaFileExtension;
static const char* areaItemsName;
static const char* missionItemsName;
static const char* settingsGroup;
......@@ -213,7 +213,7 @@ signals:
void masterControllerChanged (void);
void missionControllerChanged (void);
void visualItemsChanged (void);
void currentFileChanged ();
// void currentFileChanged ();
void dataContainerChanged ();
void readyForSaveSendChanged (bool ready);
void missionItemsChanged (void);
......@@ -230,7 +230,7 @@ signals:
void snakeCalcInProgressChanged (void);
void snakeTilesChanged (void);
void snakeTileCenterPointsChanged (void);
void snakeProgressChanged (void);
void nemoProgressChanged (void);
private:
enum SRTL_Reason {BatteryLow, UserRequest};
private slots:
......@@ -257,7 +257,7 @@ private slots:
bool _verifyScenarioDefinedWithErrorMessage (void);
void _snakeStoreWorkerResults ();
void _startStopRosBridge ();
void _initStartSnakeWorker ();
private:
void _setPhaseDistance(double distance);
......@@ -268,9 +268,12 @@ private:
void _loadCurrentMissionItemsFromBuffer();
void _saveCurrentMissionItemsToBuffer();
void _progressFromJson(JsonDocUPtr pDoc,
QNemoProgress &progress);
PlanMasterController *_masterController;
MissionController *_missionController;
QString _currentFile; // file for saveing
// QString _currentFile; // file for saveing
WimaDataContainer *_container; // container for data exchange with WimaController
QmlObjectListModel _visualItems; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
......@@ -339,14 +342,12 @@ private:
SnakeTiles _snakeTiles; // tiles
SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system
QVariantList _snakeTileCenterPoints;
QNemoProgress _snakeProgress; // measurement progress
QNemoProgress _nemoProgress; // measurement progress
ROSBridgePtr _pRosBridge;
};
void ProgressFromJson(const ROSBridge::CasePacker &casePacker, JsonDocUPtr pDoc, QNemoProgress &progress);
/*
* The following explains the structure of
* _missionController.visualItems().
......
src/Wima/WimaPlaner.cc
View file @ d02c3c46
......@@ -562,7 +562,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
_arrivalPathLength = path.size()-1;
int sequenceNumber = 0;
for (int i = 1; i < path.count()-1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1);
sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count()-1);
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
}
......@@ -576,7 +576,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
}
_returnPathLength = path.size()-1; // -1: fist item is last measurement point
for (int i = 1; i < path.count()-1; i++) {
sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count());
sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count());
_missionController->setCurrentPlanViewIndex(sequenceNumber, true);
}
......
src/comm/ros_bridge/include/CasePacker.h
View file @ d02c3c46
......@@ -30,7 +30,7 @@ public:
template<class T>
bool unpack(JsonDocUPtr &pDoc, T &msg) const {
removeTag(pDoc);
return _typeFactory->create(pDoc, msg);
return _typeFactory->create(*pDoc.get(), msg);
}
......
src/comm/ros_bridge/include/GenericMessages.h
View file @ d02c3c46
......@@ -331,6 +331,7 @@ public:
GenericProgress(){}
GenericProgress(const ContainterType<IntType> &progress) :_progress(progress){}
GenericProgress(const GenericProgress &p) :_progress(p.progress()){}
~GenericProgress() {}
virtual GenericProgress *Clone() const override { return new GenericProgress(*this); }
......
src/comm/ros_bridge/include/ROSBridge.h
View file @ d02c3c46
......@@ -28,12 +28,14 @@ public:
}
void publish(JsonDocUPtr doc);
void subscribe(const char *topic, const std::function<(JsonDocUPtr)> &callBack);
void subscribe(const char *topic, const std::function<void(JsonDocUPtr)> &callBack);
const CasePacker *casePacker() const;
//! @brief Start ROS bridge.
void start();
void stop();
//! @brief Reset ROS bridge.
void reset();
private:
TypeFactory _typeFactory;
......
src/comm/ros_bridge/include/TopicPublisher.cpp
View file @ d02c3c46
#include "TopicPublisher.h"
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
RosbridgeWsClient *rbc) :
_running(false)
, _casePacker(casePacker)
, _rbc(rbc)
{
}
ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
{
this->reset();
}
void ROSBridge::ComPrivate::TopicPublisher::start()
{
if ( _running.load() ) // start called while thread running.
return;
_running.store(true);
_rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
_pThread.reset(new std::thread(&ROSBridge::ComPrivate::transmittLoop,
std::cref(*_casePacker),
std::ref(*_rbc),
std::ref(_queue),
std::ref(_queueMutex),
std::ref(_advertisedTopicsHashList),
std::cref(_running)));
}
void ROSBridge::ComPrivate::TopicPublisher::reset()
{
if ( !_running.load() ) // stop called while thread not running.
return;
_running.store(false);
if ( !_pThread )
return;
_pThread->join();
_pThread.reset();
_rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
_queue.clear();
_advertisedTopicsHashList.clear();
}
void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacker,
RosbridgeWsClient &rbc,
ROSBridge::ComPrivate::JsonQueue &queue,
std::mutex &queueMutex,
HashSet &advertisedTopicsHashList,
const std::atomic<bool> &stopFlag)
const std::atomic<bool> &running)
{
rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
while(!stopFlag.load()){
while(running.load()){
// Pop message from queue.
queueMutex.lock();
//std::cout << "Queue size: " << queue.size() << std::endl;
......@@ -55,43 +99,3 @@ void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacke
} // while loop
}
ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
RosbridgeWsClient *rbc) :
_stopFlag(true)
, _casePacker(casePacker)
, _rbc(rbc)
{
}
ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
{
this->stop();
}
void ROSBridge::ComPrivate::TopicPublisher::start()
{
if ( !_stopFlag.load() ) // start called while thread running.
return;
_stopFlag.store(false);
_rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
_pThread.reset(new std::thread(&ROSBridge::ComPrivate::transmittLoop,
std::cref(*_casePacker),
std::ref(*_rbc),
std::ref(_queue),
std::ref(_queueMutex),
std::ref(_advertisedTopicsHashList),
std::cref(_stopFlag)));
}
void ROSBridge::ComPrivate::TopicPublisher::stop()
{
if ( _stopFlag.load() ) // start called while thread not running.
return;
_stopFlag.store(true);
if ( !_pThread )
return;
_pThread->join();
_pThread.reset();
_rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
}
src/comm/ros_bridge/include/TopicPublisher.h
View file @ d02c3c46
......@@ -25,8 +25,12 @@ public:
~TopicPublisher();
//! @brief Starts the publisher.
void start();
void stop();
//! @brief Resets the publisher.
void reset();
template<class T>
void publish(const T &msg, const std::string &topic){
JsonDocUPtr docPtr(_casePacker->pack(msg, topic));
......@@ -40,7 +44,7 @@ public:
private:
JsonQueue _queue;
std::mutex _queueMutex;
std::atomic<bool> _stopFlag;
std::atomic<bool> _running;
CasePacker *_casePacker;
ThreadPtr _pThread;
RosbridgeWsClient *_rbc;
......@@ -54,7 +58,7 @@ void transmittLoop(const ROSBridge::CasePacker &casePacker,
ROSBridge::ComPrivate::JsonQueue &queue,
std::mutex &queueMutex,
HashSet &advertisedTopicsHashList,
const std::atomic<bool> &stopFlag);
const std::atomic<bool> &running);
} // namespace CommunicatorDetail
......
src/comm/ros_bridge/include/TopicSubscriber.cpp
View file @ d02c3c46
......@@ -6,32 +6,64 @@ ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
RosbridgeWsClient *rbc) :
_casePacker(casePacker)
, _rbc(rbc)
, _running(false)
{
}
ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
{
this->reset();
}
void ROSBridge::ComPrivate::TopicSubscriber::start()
{
_rbc->addClient(ROSBridge::ComPrivate::_topicSubscriberKey);
_running = true;
}
void ROSBridge::ComPrivate::TopicSubscriber::stop()
void ROSBridge::ComPrivate::TopicSubscriber::reset()
{
_rbc->removeClient(ROSBridge::ComPrivate::_topicSubscriberKey);
if ( _running ){
for (std::string clientName : _clientList)
_rbc->removeClient(clientName);
_running = false;
_callbackMap.clear();
_clientList.clear();
}
}
bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
const char *topic,
const std::function<void(JsonDocUPtr)> &callback)
{
using namespace std::placeholders;
HashType hash = getHash(topic);
if ( !_running )
return false;
std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
+ std::string(topic);
_clientList.push_back(clientName);
HashType hash = getHash(clientName);
auto ret = _callbackMap.insert(std::make_pair(hash, callback)); //
if ( !ret.second )
return false; // Topic subscription already present.
auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback, hash, _callbackMap, _1, _2);
_rbc->subscribe(ROSBridge::ComPrivate::_topicSubscriberKey, topic, f);
{
using namespace std::placeholders;
auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
hash,
std::cref(_callbackMap),
_1, _2);
// std::cout << std::endl;
// std::cout << "topic subscription" << std::endl;
// std::cout << "client name: " << clientName << std::endl;
// std::cout << "topic: " << topic << std::endl;
_rbc->addClient(clientName);
_rbc->subscribe(clientName,
topic,
f );
}
return true;
}
......@@ -78,3 +110,13 @@ void ROSBridge::ComPrivate::subscriberCallback(
return;
}
void ROSBridge::ComPrivate::test(
std::shared_ptr<WsClient::Connection>,
std::shared_ptr<WsClient::InMessage> in_message)
{
std::cout << "test: Json document: "
<< in_message->string()
<< std::endl;
}
src/comm/ros_bridge/include/TopicSubscriber.h
View file @ d02c3c46
......@@ -10,15 +10,20 @@ namespace ComPrivate {
class TopicSubscriber
{
typedef std::vector<std::string> ClientList;
public:
typedef std::function<void(JsonDocUPtr)> CallbackType;
typedef std::map<HashType, CallbackType> CallbackMap;
TopicSubscriber() = delete;
TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc);
~TopicSubscriber();
//! @brief Starts the subscriber.
void start();
void stop();
//! @brief Resets the subscriber.
void reset();
//! @return Returns false if a subscription to this topic allready exists.
//!
......@@ -29,6 +34,8 @@ private:
CasePacker *_casePacker;
RosbridgeWsClient *_rbc;
CallbackMap _callbackMap;
ClientList _clientList;
bool _running;
};
void subscriberCallback(const HashType &hash,
......@@ -36,5 +43,7 @@ void subscriberCallback(const HashType &hash,
std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message);
}
}
void test(std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message);
} // namespace ComPrivate
} // namespace ROSBridge
src/comm/ros_bridge/src/ROSBridge.cpp
View file @ d02c3c46
......@@ -30,9 +30,9 @@ void ROSBridge::ROSBridge::start()
_topicSubscriber.start();
}
void ROSBridge::ROSBridge::stop()
void ROSBridge::ROSBridge::reset()
{
_topicPublisher.stop();
_topicSubscriber.stop();
_topicPublisher.reset();
_topicSubscriber.reset();
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment