SnakeDataManager.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) {
    // ROS Bridge.
    WimaSettings *wimaSettings =
        qgcApp()->toolbox()->settingsManager()->wimaSettings();
    auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
    auto setConnectionString = [connectionStringFact, this] {
      auto connectionString = connectionStringFact->rawValue().toString();
      if (ros_bridge::isValidConnectionString(
              connectionString.toLocal8Bit().data())) {
        this->pRosBridge.reset(
            new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
      } else {
        QString defaultString("localhost:9090");
        qgcApp()->showMessage("ROS Bridge connection string invalid: " +
                              connectionString);
        qgcApp()->showMessage("Resetting connection string to: " +
                              defaultString);
        connectionStringFact->setRawValue(
            QVariant(defaultString)); // calls this function recursively
      }
    };
    connect(connectionStringFact, &SettingsFact::rawValueChanged,
            setConnectionString);
    setConnectionString();

    // Periodic.
    connect(&this->eventTimer, &QTimer::timeout, [this] {
      if (this->rosBridgeEnabeled) {
        if (!this->pRosBridge->isRunning()) {
          this->pRosBridge->start();
        } else if (this->pRosBridge->isRunning() &&
                   this->pRosBridge->connected() &&
                   !this->topicServiceSetupDone) {
          if (this->doTopicServiceSetup())
            this->topicServiceSetupDone = true;
        } else if (this->pRosBridge->isRunning() &&
                   !this->pRosBridge->connected() &&
                   this->topicServiceSetupDone) {
          this->pRosBridge->reset();
          this->pRosBridge->start();
          this->topicServiceSetupDone = false;
        }
      } else if (this->pRosBridge->isRunning()) {
        this->pRosBridge->reset();
        this->topicServiceSetupDone = false;
      }
    });
    this->eventTimer.start(EVENT_TIMER_INTERVAL);
  }

  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::doTopicServiceSetup() {
  using namespace ros_bridge::messages;
  UniqueLock lk(this->mutex);

  if (this->tilesENU.polygons().size() == 0)
    return false;

  // Publish snake origin.
  this->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(this->ENUOrigin, *jOrigin,
                                                jOrigin->GetAllocator());
  Q_ASSERT(ret);
  (void)ret;
  this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");

  // Publish snake tiles.
  this->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(
      this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
  Q_ASSERT(ret);
  (void)ret;
  this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");

  // Subscribe nemo progress.
  this->pRosBridge->subscribe(
      "/nemo/progress",
      /* callback */ [this](JsonDocUPtr pDoc) {
        UniqueLock lk(this->mutex);
        int requiredSize = this->tilesENU.polygons().size();
        auto &progressMsg = this->qProgress;
        if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
            progressMsg.progress().size() !=
                requiredSize) { // Some error occured.
          // Set progress to default.
          progressMsg.progress().fill(0, requiredSize);

          // Publish snake origin.
          JsonDocUPtr jOrigin(
              std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
          bool ret = geographic_msgs::geo_point::toJson(
              this->ENUOrigin, *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->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
          Q_ASSERT(ret);
          (void)ret;
          this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
        }
        this->progress.clear();
        for (const auto &p : progressMsg.progress()) {
          this->progress.push_back(p);
        }
        lk.unlock();

        emit this->parent->nemoProgressChanged();
      });

  // Subscribe /nemo/heartbeat.
  this->pRosBridge->subscribe(
      "/nemo/heartbeat",
      /* callback */ [this](JsonDocUPtr pDoc) {
        UniqueLock lk(this->mutex);
        if (this->timeout.isActive()) {
          this->timeout.stop();
        }
        auto &heartbeatMsg = this->heartbeat;
        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
          heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
        }
        this->timeout.singleShot(TIMEOUT, [this] {
          UniqueLock lk(this->mutex);
          this->heartbeat.setStatus(integral(NemoStatus::Timeout));
          emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
        });
        emit this->parent->nemoStatusChanged(heartbeatMsg.status());
      });

  // Advertise /snake/get_origin.
  this->pRosBridge->advertiseService(
      "/snake/get_origin", "snake_msgs/GetOrigin",
      [this](JsonDocUPtr) -> JsonDocUPtr {
        using namespace ros_bridge::messages;
        SharedLock lk(this->mutex);

        JsonDocUPtr pDoc(
            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
        auto &origin = this->ENUOrigin;
        rapidjson::Value jOrigin(rapidjson::kObjectType);
        bool ret = geographic_msgs::geo_point::toJson(origin, jOrigin,
                                                      pDoc->GetAllocator());
        lk.unlock();
        Q_ASSERT(ret);
        (void)ret;
        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
        return pDoc;
      });

  // Advertise /snake/get_tiles.
  this->pRosBridge->advertiseService(
      "/snake/get_tiles", "snake_msgs/GetTiles",
      [this](JsonDocUPtr) -> JsonDocUPtr {
        SharedLock lk(this->mutex);

        JsonDocUPtr pDoc(
            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
        bool ret = jsk_recognition_msgs::polygon_array::toJson(
            this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
        Q_ASSERT(ret);
        (void)ret;
        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
        return pDoc;
      });

  return true;
}

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);
  }
}