SnakeDataManager mod

qgroundcontrol.pro

@@ -441,7 +441,7 @@ HEADERS += \
     src/Wima/Snake/SnakeTileLocal.h \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
-    src/Wima/Snake/SnakeW.h \
+    src/Wima/Snake/SnakeDataManager.h \
     src/Wima/WaypointManager/AreaInterface.h \
     src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \

src/Snake/snake.cpp

@@ -1041,7 +1041,6 @@ bool flight_plan::route(const BoostPolygon &area,
         route_idx.push_back(manager.IndexToNode(index).value());
     }
 
-
     // Helper Lambda.
     auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
                               std::vector<BoostPoint> &path){
@@ -1054,17 +1053,31 @@ bool flight_plan::route(const BoostPolygon &area,
         size_t idx0 = route_idx[i];
         size_t idx1 = route_idx[i+1];
         const auto &info1 = transectInfoList[idx0];
-        const auto &info2 = transectInfoList[idx0];
-        if ( info.front ){
-            const auto &t = transects[info.index];
-            for ( auto it = t.begin(); it < t.end()-1; ++it){
-                route.push_back(*it);
+        const auto &info2 = transectInfoList[idx1];
+        if (info1.index == info2.index) { // same transect?
+            if ( !info1.front ) { // transect reversal needed?
+                BoostLineString reversedTransect;
+                const auto &t = transects[info1.index];
+                for ( auto it = t.end()-1; it >= t.begin(); --it){
+                    reversedTransect.push_back(*it);
+                }
+                transectsRouted.push_back(reversedTransect);
+                for (auto it = reversedTransect.begin(); it < reversedTransect.end()-1; ++it){
+                    route.push_back(*it);
+                }
+            } else {
+                const auto &t = transects[info1.index];
+                for ( auto it = t.begin(); it < t.end()-1; ++it){
+                    route.push_back(*it);
+                }
+                transectsRouted.push_back(t);
             }
-            transectsRouted.push_back(t);
         } else {
             std::vector<size_t> idxList;
             shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
-            idxList.pop_back();
+            if ( i != route_idx.size()-2){
+                idxList.pop_back();
+            }
             idx2Vertex(idxList, route);
         }
     }

src/Snake/snake_typedefs.h

@@ -22,8 +22,21 @@ typedef bg::model::linestring<BoostPoint>               BoostLineString;
 typedef std::vector<std::vector<int64_t>>               Int64Matrix;
 typedef bg::model::box<BoostPoint>                      BoostBox;
 
-typedef bu::quantity<bu::si::length, double> Length;
-typedef bu::quantity<bu::si::area, double> Area;
-typedef bu::quantity<bu::si::plane_angle, double> Angle;
+typedef bu::quantity<bu::si::length> Length;
+typedef bu::quantity<bu::si::area> Area;
+typedef bu::quantity<bu::si::plane_angle> Angle;
 
 }
+
+namespace boost{
+namespace geometry{
+namespace model {
+bool operator==(snake::BoostPoint p1, snake::BoostPoint p2){
+    return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
+}
+bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2){
+    return !(p1 == p2);
+}
+} // namespace model
+} // namespace geometry
+} // namespace boost

src/Wima/Snake/SnakeDataManager.h

@@ -8,7 +8,7 @@
 #include "QNemoProgress.h"
 #include "QNemoHeartbeat.h"
 
-#include <boost/units/systems/si.hpp>>
+#include <boost/units/systems/si.hpp>
 
 
 using namespace boost::units;
@@ -17,6 +17,13 @@ using Area = quantity<si::area>;
 
 class SnakeImpl;
 
+enum class NemoStatus{
+    NotConnected = 0,
+    Connected = 1,
+    Timeout = -1,
+    InvalidHeartbeat = -2
+};
+
 class SnakeDataManager : public QThread{
     Q_OBJECT
 
@@ -31,8 +38,8 @@ public:
     void setServiceArea         (const QList<QGeoCoordinate>  &serviceArea);
     void setCorridor            (const QList<QGeoCoordinate>  &corridor);
 
-    QNemoProgress progress();
-    QNemoHeartbeat heartbeat();
+    QNemoProgress nemoProgress();
+    int nemoStatus();
     bool calcInProgress();
 
     Length lineDistance() const;
@@ -50,14 +57,17 @@ public:
     Length tileWidth() const;
     void setTileWidth(Length tileWidth);
 
+    void enableRosBridge();
+    void disableRosBride();
 
+signals:
+    void nemoProgressChanged();
+    void nemoStatusChanged(int status);
+    void calcInProgressChanged(bool inProgress);
 
 protected:
     void run() override;
-
 private:
-    bool precondition() const;
-    void resetWaypointData();
     SnakeImplPtr pImpl;
 };
 

src/Wima/WimaController.cc

@@ -2,14 +2,6 @@
 
 #include "utilities.h"
 
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
-#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
-#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
-#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
-#include "ros_bridge/include/messages/nemo_msgs/progress.h"
-#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
-
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
 
@@ -45,10 +37,10 @@ const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLengt
 WimaController::StatusMap WimaController::_nemoStatusMap{
     std::make_pair<int, QString>(0, "No Heartbeat"),
     std::make_pair<int, QString>(1, "Connected"),
-    std::make_pair<int, QString>(-1, "Timeout")};
+    std::make_pair<int, QString>(-1, "Timeout"),
+    std::make_pair<int, QString>(-2, "Error")};
 
 using namespace snake;
-using namespace snake_geometry;
 
 WimaController::WimaController(QObject *parent)
     : QObject                   (parent)
@@ -125,11 +117,19 @@ WimaController::WimaController(QObject *parent)
     // Snake Worker Thread.
     connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults);
     connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
-    connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit);
+    connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeDataManager::quit);
 
     // Snake.
-    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
-    _initStartSnakeWorker();
+    auto configRosBridge = [this]{
+        if ( this->_enableSnake.rawValue().toBool() ){
+            this->_snakeDataManager.enableRosBridge();
+        } else {
+            this->_snakeDataManager.disableRosBride();
+        }
+    };
+    connect(&_enableSnake, &Fact::rawValueChanged, configRosBridge);
+    configRosBridge();
+
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
     _switchSnakeManager(_enableSnake.rawValue());
 }
@@ -202,8 +202,9 @@ Fact *WimaController::altitude() {
 
 QmlObjectListModel *WimaController::snakeTiles()
 {
+    static QmlObjectListModel list;
     qWarning() << "using snake tile dummy";
-    return QmlObjectListModel();
+    return &list;
 }
 
 QVariantList WimaController::snakeTileCenterPoints()
@@ -212,11 +213,10 @@ QVariantList WimaController::snakeTileCenterPoints()
     return QVariantList();
 }
 
-QVector<int> WimaController::nemoProgress() {
-    if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
-        return _nemoProgress.progress();
-    else
-        return QVector<int>(_snakeTileCenterPoints.size(), 0);
+QVector<int> WimaController::nemoProgress()
+{
+    qWarning() << "using nemoProgress dummy";
+    return QVector<int>();
 }
 
 double WimaController::phaseDistance() const
@@ -231,17 +231,19 @@ double WimaController::phaseDuration() const
 
 int WimaController::nemoStatus() const
 {
-    return _nemoHeartbeat.status();
+    qWarning() << "using nemoStatus dummy";
+    return 0;
 }
 
 QString WimaController::nemoStatusString() const
 {
-    return _nemoStatusMap.at(_nemoHeartbeat.status());
+    return _nemoStatusMap.at(nemoStatus());
 }
 
 bool WimaController::snakeCalcInProgress() const
 {
-    return _snakeCalcInProgress;
+    qWarning() << "using snakeCalcInProgress dummy";
+    return false;
 }
 
 void WimaController::setMasterController(PlanMasterController *masterC)
@@ -762,28 +764,6 @@ void WimaController::_eventTimerHandler()
         emit WimaController::nemoStatusChanged();
         emit WimaController::nemoStatusStringChanged();
     }
-
-    if ( _snakeTicker.ready() ) {
-        if ( _enableSnake.rawValue().toBool() ) {
-            if ( !_pRosBridge->isRunning()) {
-                _pRosBridge->start();
-            } else if ( _pRosBridge->isRunning()
-                        && _pRosBridge->connected()
-                        && !_topicServiceSetupDone) {
-                if ( _doTopicServiceSetup() )
-                    _topicServiceSetupDone = true;
-            } else if ( _pRosBridge->isRunning()
-                        && !_pRosBridge->connected()
-                        && _topicServiceSetupDone){
-                _pRosBridge->reset();
-                _pRosBridge->start();
-                _topicServiceSetupDone = false;
-            }
-        } else  if ( _pRosBridge->isRunning() ) {
-                _pRosBridge->reset();
-                _topicServiceSetupDone = false;
-        }
-    }
 }
 
 void WimaController::_smartRTLCleanUp(bool flying)
@@ -965,114 +945,4 @@ void WimaController::_switchSnakeManager(QVariant variant)
     }
 }
 
-bool WimaController::_doTopicServiceSetup()
-{
-    using namespace ros_bridge::messages;
-
-    if ( _snakeTilesLocal.polygons().size() == 0)
-        return false;
-
-    // Publish snake origin.
-    _pRosBridge->advertiseTopic("/snake/origin",
-                                geographic_msgs::geo_point::messageType().c_str());
-    JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-    bool ret = geographic_msgs::geo_point::toJson(
-                _snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator());
-    Q_ASSERT(ret);
-    (void)ret;
-    _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-
-
-    // Publish snake tiles.
-    _pRosBridge->advertiseTopic("/snake/tiles",
-                                jsk_recognition_msgs::polygon_array::messageType().c_str());
-    JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-    ret = jsk_recognition_msgs::polygon_array::toJson(
-                _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
-    Q_ASSERT(ret);
-    (void)ret;
-    _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-
-
-    // Subscribe nemo progress.
-    _pRosBridge->subscribe("/nemo/progress",
-                           /* callback */ [this](JsonDocUPtr pDoc){
-        int requiredSize = this->_snakeTilesLocal.polygons().size();
-        auto& progress_msg = this->_nemoProgress;
-        if ( !nemo_msgs::progress::fromJson(*pDoc, progress_msg)
-             || progress_msg.progress().size() != requiredSize ) { // Some error occured.
-            // Set progress to default.
-            progress_msg.progress().fill(0, requiredSize);            
-
-            // Publish snake origin.
-            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            bool ret = geographic_msgs::geo_point::toJson(
-                         this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
-            Q_ASSERT(ret);
-            (void)ret;
-            this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-
-            // Publish snake tiles.
-            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            ret = jsk_recognition_msgs::polygon_array::toJson(
-                         this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
-            Q_ASSERT(ret);
-            (void)ret;
-            this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-        }
-
-        emit WimaController::nemoProgressChanged();
-    });
-
-
-    // Subscribe /nemo/heartbeat.
-    _pRosBridge->subscribe("/nemo/heartbeat",
-                           /* callback */ [this](JsonDocUPtr pDoc){
-        auto &heartbeat_msg = this->_nemoHeartbeat;
-        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) {
-            if ( heartbeat_msg.status() == this->_fallbackStatus )
-                return;
-            heartbeat_msg.setStatus(this->_fallbackStatus);
-        }
 
-        this->_nemoTimeoutTicker.reset();
-        this->_fallbackStatus = -1; /*Timeout*/
-        emit WimaController::nemoStatusChanged();
-        emit WimaController::nemoStatusStringChanged();
-    });
-
-
-    // Advertise /snake/get_origin.
-    _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
-                                  [this](JsonDocUPtr) -> JsonDocUPtr
-    {
-        using namespace ros_bridge::messages;
-
-        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        ::GeoPoint3D origin = this->_snakeOrigin;
-        rapidjson::Value jOrigin(rapidjson::kObjectType);
-        bool ret = geographic_msgs::geo_point::toJson(
-                    origin, jOrigin, pDoc->GetAllocator());
-        Q_ASSERT(ret);
-        (void)ret;
-        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
-        return pDoc;
-    });
-
-
-    // Advertise /snake/get_tiles.
-    _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
-                                  [this](JsonDocUPtr) -> JsonDocUPtr
-    {
-        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
-        bool ret = jsk_recognition_msgs::polygon_array::toJson(
-                    this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
-        Q_ASSERT(ret);
-        (void)ret;
-        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
-        return pDoc;
-    });
-
-    return true;
-}