SnakeDataManager_old.cc

#include "SnakeDataManager.h"

#include <QGeoCoordinate>
#include <QMutexLocker>

#include "QGCApplication.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"

#include <memory>
#include <mutex>
#include <shared_mutex>

#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.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/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"

#define EVENT_TIMER_INTERVAL 500 // ms
#define TIMEOUT 3000             // ms

using QVariantList = QList<QVariant>;
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;

using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;

class SnakeDataManager::SnakeImpl {
public:
  SnakeImpl(SnakeDataManager *p)
      : rosBridgeEnabeled(false), topicServiceSetupDone(false),
        lineDistance(1 * si::meter), minTransectLength(1 * si::meter),
        calcInProgress(false), parent(p) {

  }

  bool precondition() const;
  void resetOutput();
  bool doTopicServiceSetup();

  // Private data.
  ROSBridgePtr pRosBridge;
  bool rosBridgeEnabeled;
  bool topicServiceSetupDone;
  QTimer eventTimer;
  QTimer timeout;
  mutable std::shared_timed_mutex mutex;

  // Scenario
  snake::Scenario scenario;
  Length lineDistance;
  Length minTransectLength;
  QList<QGeoCoordinate> mArea;
  QList<QGeoCoordinate> sArea;
  QList<QGeoCoordinate> corridor;
  QGeoCoordinate ENUOrigin;
  SnakeTiles tiles;
  QmlObjectListModel tilesQml;
  QVariantList tileCenterPoints;
  SnakeTilesLocal tilesENU;
  QVector<QPointF> tileCenterPointsENU;

  // Waypoints
  QVector<QGeoCoordinate> waypoints;
  QVector<QGeoCoordinate> arrivalPath;
  QVector<QGeoCoordinate> returnPath;
  QVector<QPointF> waypointsENU;
  QVector<QPointF> arrivalPathENU;
  QVector<QPointF> returnPathENU;
  QString errorMessage;

  // Other
  std::atomic_bool calcInProgress;
  QNemoProgress qProgress;
  std::vector<int> progress;
  QNemoHeartbeat heartbeat;
  SnakeDataManager *parent;
};



bool SnakeDataManager::SnakeImpl::precondition() const { return true; }

//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeDataManager::SnakeImpl::resetOutput() {
  this->waypoints.clear();
  this->arrivalPath.clear();
  this->returnPath.clear();
  this->ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
  this->waypointsENU.clear();
  this->arrivalPathENU.clear();
  this->returnPathENU.clear();
  this->tilesQml.clearAndDeleteContents();
  this->tiles.polygons().clear();
  this->tileCenterPoints.clear();
  this->tilesENU.polygons().clear();
  this->tileCenterPointsENU.clear();
  this->errorMessage.clear();
}

template <class Callable> class CommandRAII {
public:
  CommandRAII(Callable &fun) : _fun(fun) {}

  ~CommandRAII() { _fun(); }

private:
  Callable _fun;
};

SnakeDataManager::SnakeDataManager(QObject *parent)
    : QThread(parent), pImpl(std::make_unique<SnakeImpl>(this))

{}

SnakeDataManager::~SnakeDataManager() {}

void SnakeDataManager::setMeasurementArea(
    const QList<QGeoCoordinate> &measurementArea) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->mArea = measurementArea;
  for (auto &vertex : this->pImpl->mArea) {
    vertex.setAltitude(0);
  }
}

void SnakeDataManager::setServiceArea(
    const QList<QGeoCoordinate> &serviceArea) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->sArea = serviceArea;
  for (auto &vertex : this->pImpl->sArea) {
    vertex.setAltitude(0);
  }
}

void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->corridor = corridor;
  for (auto &vertex : this->pImpl->corridor) {
    vertex.setAltitude(0);
  }
}

const QmlObjectListModel *SnakeDataManager::tiles() const {
  SharedLock lk(this->pImpl->mutex);
  return &this->pImpl->tilesQml;
}

QVariantList SnakeDataManager::tileCenterPoints() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->tileCenterPoints;
}

QNemoProgress SnakeDataManager::nemoProgress() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->qProgress;
}

int SnakeDataManager::nemoStatus() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->heartbeat.status();
}

bool SnakeDataManager::calcInProgress() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->calcInProgress.load();
}

QString SnakeDataManager::errorMessage() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->errorMessage;
}

bool SnakeDataManager::success() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->errorMessage.isEmpty() ? true : false;
}

QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->waypoints;
}

QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->arrivalPath;
}

QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->returnPath;
}

Length SnakeDataManager::lineDistance() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->lineDistance;
}

void SnakeDataManager::setLineDistance(Length lineDistance) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->lineDistance = lineDistance;
}

Area SnakeDataManager::minTileArea() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->scenario.minTileArea();
}

void SnakeDataManager::setMinTileArea(Area minTileArea) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->scenario.setMinTileArea(minTileArea);
}

Length SnakeDataManager::tileHeight() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->scenario.tileHeight();
}

void SnakeDataManager::setTileHeight(Length tileHeight) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->scenario.setTileHeight(tileHeight);
}

Length SnakeDataManager::tileWidth() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->scenario.tileWidth();
}

void SnakeDataManager::setTileWidth(Length tileWidth) {
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->scenario.setTileWidth(tileWidth);
}

void SnakeDataManager::enableRosBridge() {
  this->pImpl->rosBridgeEnabeled = true;
}

void SnakeDataManager::disableRosBride() {
  this->pImpl->rosBridgeEnabeled = false;
}

void SnakeDataManager::run() {
#ifndef NDEBUG
  auto startTime = std::chrono::high_resolution_clock::now();
  qDebug() << "SnakeDataManager::run()";
#endif

  this->pImpl->calcInProgress.store(true);
  emit calcInProgressChanged(this->pImpl->calcInProgress.load());
  auto onExit = [this, &startTime] {
#ifndef NDEBUG
    qDebug() << "SnakeDataManager::run() execution time: "
             << std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::high_resolution_clock::now() - startTime)
                    .count()
             << " ms";
#endif
    this->pImpl->calcInProgress.store(false);
    emit calcInProgressChanged(this->pImpl->calcInProgress.load());
  };
  CommandRAII<decltype(onExit)> onExitRAII(onExit);

  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->resetOutput();

  if (!this->pImpl->precondition())
    return;

  if (this->pImpl->mArea.size() < 3) {
    this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
    return;
  }
  if (this->pImpl->sArea.size() < 3) {
    this->pImpl->errorMessage = "Service area invalid: size < 3.";
    return;
  }

  this->pImpl->ENUOrigin = this->pImpl->mArea.front();
  auto &origin = this->pImpl->ENUOrigin;
  qDebug() << "SnakeDataManager::run(): origin: " << origin.latitude() << " "
           << origin.longitude() << " " << origin.altitude();
  // Update measurement area.
  auto &mAreaEnu = this->pImpl->scenario.measurementArea();
  auto &mArea = this->pImpl->mArea;
  mAreaEnu.clear();
  for (auto geoVertex : mArea) {
    snake::BoostPoint p;
    snake::toENU(origin, geoVertex, p);
    mAreaEnu.outer().push_back(p);
  }

  // Update service area.
  auto &sAreaEnu = this->pImpl->scenario.serviceArea();
  auto &sArea = this->pImpl->sArea;
  sAreaEnu.clear();
  for (auto geoVertex : sArea) {
    snake::BoostPoint p;
    snake::toENU(origin, geoVertex, p);
    sAreaEnu.outer().push_back(p);
  }

  // Update corridor.
  auto &corridorEnu = this->pImpl->scenario.corridor();
  auto &corridor = this->pImpl->corridor;
  corridor.clear();
  for (auto geoVertex : corridor) {
    snake::BoostPoint p;
    snake::toENU(origin, geoVertex, p);
    corridorEnu.outer().push_back(p);
  }

  if (!this->pImpl->scenario.update()) {
    this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
    return;
  }

  // Store scenario data.
  {
    // Get tiles.
    const auto &tiles = this->pImpl->scenario.tiles();
    const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
    for (unsigned int i = 0; i < tiles.size(); ++i) {
      const auto &tile = tiles[i];
      SnakeTile geoTile;
      SnakeTileLocal enuTile;
      for (size_t i = tile.outer().size(); i < tile.outer().size() - 1; ++i) {
        auto &p = tile.outer()[i];
        QPointF enuVertex(p.get<0>(), p.get<1>());
        QGeoCoordinate geoVertex;
        snake::fromENU(origin, p, geoVertex);
        enuTile.polygon().points().push_back(enuVertex);
        geoTile.push_back(geoVertex);
      }
      const auto &boostPoint = centerPoints[i];
      QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
      QGeoCoordinate geoVertex;
      snake::fromENU(origin, boostPoint, geoVertex);
      geoTile.setCenter(geoVertex);
      this->pImpl->tilesQml.append(new SnakeTile(geoTile));
      this->pImpl->tiles.polygons().push_back(geoTile);
      this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
      this->pImpl->tilesENU.polygons().push_back(enuTile);
      this->pImpl->tileCenterPointsENU.push_back(enuVertex);
    }
  }

  // Create transects.
  std::string errorString;
  snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle *
                     degree::degree);
  snake::flight_plan::Transects transects;
  transects.push_back(bg::model::linestring<snake::BoostPoint>{
      this->pImpl->scenario.homePositon()});
  bool value = snake::flight_plan::transectsFromScenario(
      this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha,
      this->pImpl->scenario, this->pImpl->progress, transects, errorString);
  if (!value) {
    this->pImpl->errorMessage = errorString.c_str();
    return;
  }

  // Route transects
  snake::flight_plan::Transects transectsRouted;
  snake::flight_plan::Route route;
  value =
      snake::flight_plan::route(this->pImpl->scenario.joinedArea(), transects,
                                transectsRouted, route, errorString);
  if (!value) {
    this->pImpl->errorMessage = errorString.c_str();
    return;
  }

  // Store arrival path.
  const auto &firstWaypoint = transectsRouted.front().front();
  long startIdx = 0;
  for (long i = 0; i < long(route.size()); ++i) {
    const auto &boostVertex = route[i];
    if (boostVertex == firstWaypoint) {
      startIdx = i;
      break;
    }
    QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
    QGeoCoordinate geoVertex;
    snake::fromENU(origin, boostVertex, geoVertex);
    this->pImpl->arrivalPathENU.push_back(enuVertex);
    this->pImpl->arrivalPath.push_back(geoVertex);
  }
  // Store return path.
  long endIdx = 0;
  const auto &lastWaypoint = transectsRouted.back().back();
  for (long i = route.size() - 1; i >= 0; --i) {
    const auto &boostVertex = route[i];
    if (boostVertex == lastWaypoint) {
      endIdx = i;
      break;
    }
    QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
    QGeoCoordinate geoVertex;
    snake::fromENU(origin, boostVertex, geoVertex);
    this->pImpl->returnPathENU.push_back(enuVertex);
    this->pImpl->returnPath.push_back(geoVertex);
  }
  // Store waypoints.
  for (long i = startIdx; i <= endIdx; ++i) {
    const auto &boostVertex = route[i];
    QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
    QGeoCoordinate geoVertex;
    snake::fromENU(origin, boostVertex, geoVertex);
    this->pImpl->waypointsENU.push_back(enuVertex);
    this->pImpl->waypoints.push_back(geoVertex);
  }
}