diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index e08b969369ec838e155ed17c3e6160f69110dba7..4aae7be2fefbf14592531d2bcf43f4aeedd28d87 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 \ diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 5feaceebed1a555bf5737c74f4222b83fc968953..cbc61c3dd8e48d67ab67d32611aa2f7f2187df6c 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -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 diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index e0d16655f76bac8304356a27f07f7f38c77fc8a0..7fff83cef486457fc7f9a11e008aab61dc12a1e8 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -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 &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(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(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(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 diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 9ce1bb923e2b712f3b8c113e53a6baa326d1014d..3763f12b1b1d47531c7249f8f1d0520f4e623160 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -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 &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); diff --git a/src/Snake/snake_geometry.cpp b/src/Snake/snake_geometry.cpp index e26a5f1aca397761f88d252b16d99f037b045c7f..60044db429496b658246d888f9ff255fb5ac5c0a 100644 --- a/src/Snake/snake_geometry.cpp +++ b/src/Snake/snake_geometry.cpp @@ -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]; } diff --git a/src/Wima/QNemoProgress.cc b/src/Wima/QNemoProgress.cc index 00754c7396cb980b9a718aaea3dc813ec2affafb..d7dc1a396652cc9e5e39dac379334ecf7838f641 100644 --- a/src/Wima/QNemoProgress.cc +++ b/src/Wima/QNemoProgress.cc @@ -1,28 +1,2 @@ #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 &QNemoProgress::progress() { - emit QNemoProgress::progressChanged(); - return _progress; -} - -const QVector &QNemoProgress::progress() const { - return _progress; -} diff --git a/src/Wima/QNemoProgress.h b/src/Wima/QNemoProgress.h index bc0d57852e21526810e8e82f515ad3890802748a..1f2d5e037b9197cbd64166925af954a371c59b3e 100644 --- a/src/Wima/QNemoProgress.h +++ b/src/Wima/QNemoProgress.h @@ -5,22 +5,6 @@ #include "ros_bridge/include/GenericMessages.h" namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs; -typedef NemoMsgs::GenericProgress 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 &progress(void) const override; - virtual QVector &progress(void) override; - - signals: - void progressChanged(); -}; +typedef NemoMsgs::GenericProgress QNemoProgress; diff --git a/src/Wima/WimaControllerDetail.cc b/src/Wima/SnakeWorker.cc similarity index 97% rename from src/Wima/WimaControllerDetail.cc rename to src/Wima/SnakeWorker.cc index b0cd097f85b472a9e2d1311f30e25ed513775d33..e82bf3d64723118897d82ef45a605163e30fb806 100644 --- a/src/Wima/WimaControllerDetail.cc +++ b/src/Wima/SnakeWorker.cc @@ -1,4 +1,4 @@ -#include "WimaControllerDetail.h" +#include "SnakeWorker.h" #include @@ -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(); } diff --git a/src/Wima/WimaControllerDetail.h b/src/Wima/SnakeWorker.h similarity index 97% rename from src/Wima/WimaControllerDetail.h rename to src/Wima/SnakeWorker.h index 729efe4ea013084dd345ec05126a594b83253159..06450cec593010525daca9f29cda044c6bc716f6 100644 --- a/src/Wima/WimaControllerDetail.h +++ b/src/Wima/SnakeWorker.h @@ -28,6 +28,7 @@ class SnakeWorker : public QThread{ public: SnakeWorker(QObject *parent = nullptr); + ~SnakeWorker() override; void setScenario (const Scenario &scenario); void setProgress (const QVector &progress); @@ -36,9 +37,6 @@ public: const WorkerResult_t &getResult() const; -signals: - void resultReady(); - protected: void run() override; diff --git a/src/Wima/WaypointManager/GenericPathSlicer.cpp b/src/Wima/WaypointManager/GenericPathSlicer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..68a57444b8aa6a9701ab7af30d83f0590fa6d167 --- /dev/null +++ b/src/Wima/WaypointManager/GenericPathSlicer.cpp @@ -0,0 +1 @@ +#include "GenericPathSlicer.h" diff --git a/src/Wima/WaypointManager/GenericPathSlicer.h b/src/Wima/WaypointManager/GenericPathSlicer.h new file mode 100644 index 0000000000000000000000000000000000000000..b8f379570f163e686e49273d34ef47decbfa64eb --- /dev/null +++ b/src/Wima/WaypointManager/GenericPathSlicer.h @@ -0,0 +1,245 @@ +#pragma once + +#include +#include + +//! @brief Base class for all waypoint managers. +template class Container /*e.g. QVector*/> +class GenericPathSlicer +{ +public: + typedef Container 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 Container /*e.g. QVector*/> +GenericPathSlicer::GenericPathSlicer(): + _idxValid(false) + , _atEnd(false) +{} + +template class Container /*e.g. QVector*/> +const Container &GenericPathSlicer::path() const { + return _path; +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::setPath(const ContainerType &waypoints) { + _idxValid = false; + _path = waypoints; +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::push_back(const ElementType &wp) { + _idxValid = false; + _path.push_back(wp); +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::push_front(const ElementType &wp) { + _idxValid = false; + _path.push_front(wp); +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::clear(){ + _idxValid = false; + _path.clear(); +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::insert(int i, const ElementType &wp){ + _idxValid = false; + _path.insert(i, wp); +} + +template class Container /*e.g. QVector*/> +std::uint32_t GenericPathSlicer::size() +{ + return std::uint32_t(_path.size()); +} + +template class Container /*e.g. QVector*/> +ElementType &GenericPathSlicer::at(unsigned int i) +{ + return _path.at(i); +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::setOverlap(std::uint32_t overlap) +{ + _idxValid = false; + _overlap = overlap; +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::setN(uint32_t N) +{ + _idxValid = false; + _N = N > 0 ? N : 1; +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::setStartIndex(int idxStart) +{ + _idxValid = false; + _idxStart = idxStart; +} + +template class Container /*e.g. QVector*/> +uint32_t GenericPathSlicer::overlap() +{ + return _overlap; +} + +template class Container /*e.g. QVector*/> +uint32_t GenericPathSlicer::N() +{ + return _N; +} + +template class Container /*e.g. QVector*/> +int GenericPathSlicer::startIndex() +{ + if (!_idxValid) + _updateIdx(); + return _idxStart; +} + +template class Container /*e.g. QVector*/> +int GenericPathSlicer::endIndex() +{ + if (!_idxValid) + _updateIdx(); + return _idxEnd; +} + +template class Container /*e.g. QVector*/> +int GenericPathSlicer::nextIndex() +{ + if (!_idxValid) + _updateIdx(); + return _idxNext; +} + + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::_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 Container /*e.g. QVector*/> +void GenericPathSlicer::slice(Container &c){ + + if ( !_idxValid) + _updateIdx(); + + (void)c; + assert(false); //ToDo + + // extract waypoints + +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::next(Container &c){ + setStartIndex(_idxNext); + slice(c); +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::previous(Container &c){ + setStartIndex(_idxPrevious); + slice(c); +} + +template class Container /*e.g. QVector*/> +void GenericPathSlicer::reset(Container &c){ + setStartIndex(0); + slice(c); +} diff --git a/src/Wima/WaypointManager/GenericWaypointManager.cpp b/src/Wima/WaypointManager/GenericWaypointManager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f5819b9ce12edb10cc4554f8ae22650af0670ba6 --- /dev/null +++ b/src/Wima/WaypointManager/GenericWaypointManager.cpp @@ -0,0 +1,6 @@ +#include "GenericWaypointManager.h" + +GenericWaypointManager::GenericWaypointManager() +{ + +} diff --git a/src/Wima/WaypointManager/GenericWaypointManager.h b/src/Wima/WaypointManager/GenericWaypointManager.h new file mode 100644 index 0000000000000000000000000000000000000000..8ec74f3b8d6487964d45ff12190fbd0820a181ea --- /dev/null +++ b/src/Wima/WaypointManager/GenericWaypointManager.h @@ -0,0 +1,8 @@ +#pragma once + + +class GenericWaypointManager +{ +public: + GenericWaypointManager(); +}; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 3c5f693893c64cff319dc0cc40cc438ea9d7c2e9..ab73af92df3010d6473d56c78c6a9ce325c01121 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -17,7 +17,7 @@ #include -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 &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(); } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index a54ae208f103b3239032196820585651cd5c68b8..e99b2dc2ff41f194c32daf2948ac12c82cc8523e 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -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 *snakeProgress READ snakeProgress NOTIFY snakeProgressChanged) + Q_PROPERTY(QVector 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 *snakeProgress (void) { return &_snakeProgress.progress();} + QVector 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(). diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 57ee5a2e93dad71837411313985fd63040676798..5600c337724d6fe6de4d8710fb9a5ad2da0a4421 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -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); } diff --git a/src/comm/ros_bridge/include/CasePacker.h b/src/comm/ros_bridge/include/CasePacker.h index 5ecfc42e33fa6f2bb56d822075cc681bd49d8b14..3bf889a744f8f67a2a065fdfcfae94fe6b1e30cc 100644 --- a/src/comm/ros_bridge/include/CasePacker.h +++ b/src/comm/ros_bridge/include/CasePacker.h @@ -30,7 +30,7 @@ public: template bool unpack(JsonDocUPtr &pDoc, T &msg) const { removeTag(pDoc); - return _typeFactory->create(pDoc, msg); + return _typeFactory->create(*pDoc.get(), msg); } diff --git a/src/comm/ros_bridge/include/GenericMessages.h b/src/comm/ros_bridge/include/GenericMessages.h index a27ed74fb05f0b69acb8514a3fdb07c6173663a9..0b2d96d90f26fd9029702e83b8665a03ff7b8fee 100644 --- a/src/comm/ros_bridge/include/GenericMessages.h +++ b/src/comm/ros_bridge/include/GenericMessages.h @@ -331,6 +331,7 @@ public: GenericProgress(){} GenericProgress(const ContainterType &progress) :_progress(progress){} GenericProgress(const GenericProgress &p) :_progress(p.progress()){} + ~GenericProgress() {} virtual GenericProgress *Clone() const override { return new GenericProgress(*this); } diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h index 3c0f9eed25ce55752fd5dbcc76b99e3dd90e01d0..48b090502c2e08ab0c90c6d1a91b2cb7a3dc13ec 100644 --- a/src/comm/ros_bridge/include/ROSBridge.h +++ b/src/comm/ros_bridge/include/ROSBridge.h @@ -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 &callBack); const CasePacker *casePacker() const; + //! @brief Start ROS bridge. void start(); - void stop(); + //! @brief Reset ROS bridge. + void reset(); private: TypeFactory _typeFactory; diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp index 00260b2181ffd55d1e2f2f9ad57264d4b0e7dccd..3b8d62639e94466610fdcf6fdadef5cdeb7cabdb 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.cpp +++ b/src/comm/ros_bridge/include/TopicPublisher.cpp @@ -1,16 +1,60 @@ #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 &stopFlag) + const std::atomic &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); -} diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h index 0e3f3e1024f44760e9710f0bd95f4f1ea8cb815b..a762eafd42e01197d9431929ff28ecb29ebb96ba 100644 --- a/src/comm/ros_bridge/include/TopicPublisher.h +++ b/src/comm/ros_bridge/include/TopicPublisher.h @@ -25,8 +25,12 @@ public: ~TopicPublisher(); + //! @brief Starts the publisher. void start(); - void stop(); + + //! @brief Resets the publisher. + void reset(); + template 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 _stopFlag; + std::atomic _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 &stopFlag); + const std::atomic &running); } // namespace CommunicatorDetail diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp index c1e8f0b46f42f77ad885ebf9688126e76aa24ecd..a76971253cf90b17be7f03919a9522b11be67e15 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.cpp +++ b/src/comm/ros_bridge/include/TopicSubscriber.cpp @@ -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 &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, + std::shared_ptr in_message) +{ + + std::cout << "test: Json document: " + << in_message->string() + << std::endl; +} diff --git a/src/comm/ros_bridge/include/TopicSubscriber.h b/src/comm/ros_bridge/include/TopicSubscriber.h index ff65d2e814ef861ef5cea2a5e676863a6b1311a7..3c05edf7a3ba1e48c48a6d35abea63eef588a9bc 100644 --- a/src/comm/ros_bridge/include/TopicSubscriber.h +++ b/src/comm/ros_bridge/include/TopicSubscriber.h @@ -10,15 +10,20 @@ namespace ComPrivate { class TopicSubscriber { + typedef std::vector ClientList; public: typedef std::function CallbackType; typedef std::map 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 /*connection*/, std::shared_ptr in_message); -} -} +void test(std::shared_ptr /*connection*/, + std::shared_ptr in_message); +} // namespace ComPrivate +} // namespace ROSBridge diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp index 01cad18d557000af6a6d4ae450f9e4f666fb6ee4..f18a1da6235242f8544a4f3ad6d54f38a70df619 100644 --- a/src/comm/ros_bridge/src/ROSBridge.cpp +++ b/src/comm/ros_bridge/src/ROSBridge.cpp @@ -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(); }