NemoInterface.cpp

#include "NemoInterface.h"
#include "nemo_interface/SnakeTilesLocal.h"

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

#include <shared_mutex>

#include <QTimer>

#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/QNemoProgress.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"

QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")

#define EVENT_TIMER_INTERVAL 100 // ms
auto constexpr static timeoutInterval = std::chrono::milliseconds(3000);

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

class NemoInterface::Impl {
public:
  enum class STATE {
    STOPPED,
    RUNNING,
    WEBSOCKET_DETECTED,
    HEARTBEAT_DETECTED,
    TRY_TOPIC_SERVICE_SETUP,
    READY,
    SYNCHRONIZING,
    TIMEOUT,
    INVALID_HEARTBEAT
  }

  Impl(NemoInterface *p);

  void start();
  void stop();

  // Tile editing.
  // 	Functions that require communication to device.
  void addTiles(const TilePtrArray &tileArray);
  void addTiles(const TileArray &tileArray);
  void removeTiles(const IDArray &idArray);
  void clearTiles();

  // 	Functions that don't require communication to device.
  TileArray getTiles(const IDArray &idArray);
  TileArray getAllTiles();
  LogicalArray containsTiles(const IDArray &idArray);
  std::size_t size();
  bool empty();

  // Progress.
  ProgressArray getProgress();
  ProgressArray getProgress(const IDArray &idArray);

  NemoInterface::STATUS status();
  bool running();

private:
  typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimePoint;
  typedef std::map<long, MeasurementTile> TileMap;
  typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;

  void _addTiles(const TileArray &tileArray);
  void _removeTiles(const IDArray &idArray);
  void _clearTiles();

  void doTopicServiceSetup();
  void loop();

  bool _setState(STATE s);
  static bool _running(STATE s);
  static void _translate(STATE state, NemoInterface::STATUS &status);
  static void _translate(Heartbeat hb, STATE &state);

  TimePoint nextTimeout;
  mutable std::shared_timed_mutex timeoutMutex;

  STATE _state;
  ROSBridgePtr _pRosBridge;
  TileMap _tileMap;
  NemoInterface *_parent;
};

using StatusMap = std::map<NemoInterface::STATUS, QString>;
static StatusMap statusMap{
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::HEARTBEAT_DETECTED, "Heartbeat Detected"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::TIMEOUT, "Timeout"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};

NemoInterface::Impl::Impl(NemoInterface *p)
    : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
      running_(false), topicServiceSetupDone(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())) {
    } else {
      qgcApp()->warningMessageBoxOnMainThread(
          "Nemo Interface",
          "Websocket connection string possibly invalid: " + connectionString +
              ". Trying to connect anyways.");
    }
    this->_pRosBridge.reset(
        new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
  };
  connect(connectionStringFact, &SettingsFact::rawValueChanged,
          setConnectionString);
  setConnectionString();

  // Periodic.
  connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); });
  this->loopTimer.start(EVENT_TIMER_INTERVAL);
}

void NemoInterface::Impl::start() {
  this->running_ = true;
  emit this->_parent->runningChanged();
}

void NemoInterface::Impl::stop() {
  this->running_ = false;
  emit this->_parent->runningChanged();
}

TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
  TileArray tileArray;

  for (const auto &id : idArray) {
    const auto it = _tileMap.find(id);
    if (it != _tileMap.end()) {
      tileArray.append(it->second);
    }
  }

  return tileArray;
}

TileArray NemoInterface::Impl::getAllTiles() {
  TileArray tileArray;

  for (const auto &entry : _tileMap) {
    tileArray.append(entry.second);
  }

  return tileArray;
}

LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) {
  LogicalArray logicalArray;

  for (const auto &id : idArray) {
    const auto &it = _tileMap.find(id);
    logicalArray.append(it != _tileMap.end());
  }

  return logicalArray;
}

std::size_t NemoInterface::Impl::size() { return _tileMap.size(); }

bool NemoInterface::Impl::empty() { return _tileMap.empty(); }

ProgressArray NemoInterface::Impl::getProgress() {
  ProgressArray progressArray;

  for (const auto &entry : _tileMap) {
    progressArray.append(TaggedProgress{entry.first, entry.second.progress()});
  }

  return progressArray;
}

ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
  ProgressArray progressArray;

  for (const auto &id : idArray) {
    const auto it = _tileMap.find(id);
    if (id != _tileMap.end()) {
      progressArray.append(TaggedProgress{it->first, it->second.progress()});
    }
  }

  return progressArray;
}

void NemoInterface::Impl::setTileData(const TileData &tileData) {
  this->tileData = tileData;
  if (tileData.tiles.count() > 0) {
    std::lock(this->ENUOriginMutex, this->tilesENUMutex);
    UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
    UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);

    const auto *obj = tileData.tiles[0];
    const auto *tile = qobject_cast<const MeasurementTile *>(obj);
    if (tile != nullptr) {
      if (tile->coordinateList().size() > 0) {
        if (tile->coordinateList().first().isValid()) {
          this->ENUOrigin = tile->coordinateList().first();
          const auto &origin = this->ENUOrigin;
          this->tilesENU.polygons().clear();
          for (int i = 0; i < tileData.tiles.count(); ++i) {
            obj = tileData.tiles[i];
            tile = qobject_cast<const MeasurementTile *>(obj);
            if (tile != nullptr) {
              SnakeTileLocal tileENU;
              geometry::areaToEnu(origin, tile->coordinateList(),
                                  tileENU.path());
              this->tilesENU.polygons().push_back(std::move(tileENU));
            } else {
              qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
              break;
            }
          }
        } else {
          qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
        }
      } else {
        qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
      }
    }
  } else {
    this->tileData.clear();

    std::lock(this->ENUOriginMutex, this->tilesENUMutex);
    UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
    UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
    this->ENUOrigin = QGeoCoordinate(0, 0, 0);
    this->tilesENU = SnakeTilesLocal();
  }
}

bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
  return this->tileData == tileData;
}

void NemoInterface::Impl::publishTileData() {
  std::lock(this->ENUOriginMutex, this->tilesENUMutex);
  UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
  UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);

  if (this->tilesENU.polygons().size() > 0 && this->running_ &&
      this->topicServiceSetupDone) {
    this->publishENUOrigin();
    this->publishTilesENU();
  }
}

NemoInterface::STATUS NemoInterface::Impl::status() {
  return _translate(this->_state);
}

QVector<int> NemoInterface::Impl::progress() {
  SharedLock lk(this->progressMutex);
  return this->qProgress.progress();
}

bool NemoInterface::Impl::running() { return _running(this->_state); }

void NemoInterface::Impl::doTopicServiceSetup() {
  using namespace ros_bridge::messages;

  // snake tiles.
  {
    SharedLock lk(this->tilesENUMutex);
    this->_pRosBridge->advertiseTopic(
        "/snake/tiles",
        jsk_recognition_msgs::polygon_array::messageType().c_str());
  }

  // snake origin.
  {
    SharedLock lk(this->ENUOriginMutex);
    this->_pRosBridge->advertiseTopic(
        "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
  }

  // Subscribe nemo progress.
  this->_pRosBridge->subscribe(
      "/nemo/progress",
      /* callback */ [this](JsonDocUPtr pDoc) {
        std::lock(this->progressMutex, this->tilesENUMutex,
                  this->ENUOriginMutex);
        UniqueLock lk1(this->progressMutex, std::adopt_lock);
        UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
        UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);

        int requiredSize = this->tilesENU.polygons().size();
        auto &progressMsg = this->qProgress;
        if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
            progressMsg.progress().size() !=
                requiredSize) { // Some error occured.
          progressMsg.progress().clear();
          qgcApp()->informationMessageBoxOnMainThread(
              "Nemo Interface", "Invalid progress message received.");
        }
        emit this->_parent->progressChanged();

        lk1.unlock();
        lk2.unlock();
        lk3.unlock();
      });

  // Subscribe /nemo/heartbeat.
  this->_pRosBridge->subscribe(
      "/nemo/heartbeat",
      /* callback */ [this](JsonDocUPtr pDoc) {
        //        auto start = std::chrono::high_resolution_clock::now();
        nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
          this->_setState(STATUS::INVALID_HEARTBEAT);
        } else {
          this->_setState(heartbeatToStatus(heartbeatMsg));
        }
        if (this->status_ == STATUS::INVALID_HEARTBEAT) {
          UniqueLock lk(this->timeoutMutex);
          this->nextTimeout = TimePoint::max();
        } else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
          UniqueLock lk(this->timeoutMutex);
          this->nextTimeout =
              std::chrono::high_resolution_clock::now() + timeoutInterval;
        }

        //        auto delta =
        //        std::chrono::duration_cast<std::chrono::milliseconds>(
        //            std::chrono::high_resolution_clock::now() - start);
        //        std::cout << "/nemo/heartbeat callback time: " <<
        //        delta.count() << " ms"
        //                  << std::endl;
      });

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

        JsonDocUPtr pDoc(
            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
        auto &origin = this->ENUOrigin;
        rapidjson::Value jOrigin(rapidjson::kObjectType);
        lk.unlock();
        if (geographic_msgs::geo_point::toJson(origin, jOrigin,
                                               pDoc->GetAllocator())) {
          lk.unlock();
          pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
        } else {
          lk.unlock();
          qCWarning(NemoInterfaceLog)
              << "/snake/get_origin service: could not create json document.";
        }

        return pDoc;
      });

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

        JsonDocUPtr pDoc(
            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);

        if (jsk_recognition_msgs::polygon_array::toJson(
                this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
          lk.unlock();
          pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
        } else {
          lk.unlock();
          qCWarning(NemoInterfaceLog)
              << "/snake/get_tiles service: could not create json document.";
        }

        return pDoc;
      });
}

void NemoInterface::Impl::loop() {
  // Check ROS Bridge status and do setup if necessary.
  if (this->running_) {
    if (!this->_pRosBridge->isRunning()) {
      this->_pRosBridge->start();
      this->loop();
    } else if (this->_pRosBridge->isRunning() &&
               this->_pRosBridge->connected() && !this->topicServiceSetupDone) {
      this->doTopicServiceSetup();
      this->topicServiceSetupDone = true;

      this->_setState(STATUS::WEBSOCKET_DETECTED);
    } else if (this->_pRosBridge->isRunning() &&
               !this->_pRosBridge->connected() && this->topicServiceSetupDone) {
      this->_pRosBridge->reset();
      this->_pRosBridge->start();
      this->topicServiceSetupDone = false;

      this->_setState(STATUS::TIMEOUT);
    }
  } else if (this->_pRosBridge->isRunning()) {
    this->_pRosBridge->reset();
    this->topicServiceSetupDone = false;
  }

  // Check if heartbeat timeout occured.
  if (this->running_ && this->topicServiceSetupDone) {
    UniqueLock lk(this->timeoutMutex);
    if (this->nextTimeout != TimePoint::max() &&
        this->nextTimeout < std::chrono::high_resolution_clock::now()) {
      lk.unlock();
      if (this->_pRosBridge->isRunning() && this->_pRosBridge->connected()) {
        this->_setState(STATUS::WEBSOCKET_DETECTED);
      } else {
        this->_setState(STATUS::TIMEOUT);
      }
    }
  }
}

NemoInterface::STATUS NemoInterface::Impl::heartbeatToStatus(
    const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb) {
  if (STATUS(hb.status()) == STATUS::HEARTBEAT_DETECTED)
    return STATUS::HEARTBEAT_DETECTED;
  else
    return STATUS::INVALID_HEARTBEAT;
}

void NemoInterface::Impl::publishTilesENU() {
  using namespace ros_bridge::messages;
  JsonDocUPtr jSnakeTiles(
      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
  if (jsk_recognition_msgs::polygon_array::toJson(
          this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
    this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
  } else {
    qCWarning(NemoInterfaceLog)
        << "Impl::publishTilesENU: could not create json document.";
  }
}

void NemoInterface::Impl::publishENUOrigin() {
  using namespace ros_bridge::messages;
  JsonDocUPtr jOrigin(
      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
  if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
                                         jOrigin->GetAllocator())) {
    this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
  } else {
    qCWarning(NemoInterfaceLog)
        << "Impl::publishENUOrigin: could not create json document.";
  }
}

bool NemoInterface::Impl::_setState(STATE s) {
  if (s != this->status_) {
    this->status_ = s;
    emit this->_parent->statusChanged();
    return true;
  } else {
    return false;
  }
}

// ===============================================================
// NemoInterface
NemoInterface::NemoInterface()
    : QObject(), pImpl(std::make_unique<NemoInterface::Impl>(this)) {}

NemoInterface *NemoInterface::createInstance() { return new NemoInterface(); }

NemoInterface *NemoInterface::instance() {
  return GenericSingelton<NemoInterface>::instance(
      NemoInterface::createInstance);
}

NemoInterface::~NemoInterface() {}

void NemoInterface::start() { this->pImpl->start(); }

void NemoInterface::stop() { this->pImpl->stop(); }

void NemoInterface::addTiles(const NemoInterface::TilePtrArray &tileArray) {
  this->pImpl -
}

void NemoInterface::addTiles(const TileArray &tileArray) {
  TilePtrArray ptrArray;
  for (const auto &tile : tileArray) {
    ptrArray.append(&tile);
  }
  addTiles(ptrArray);
}

void NemoInterface::removeTiles(const IDArray &idArray) {
  this->pImpl->removeTiles(idArray);
}

void NemoInterface::clearTiles() { this->pImpl->clearTiles(); }

TileArray NemoInterface::getTiles(const IDArray &idArray) {
  return this->pImpl->getTiles(idArray);
}

TileArray NemoInterface::getAllTiles() {
  return this->pImpl->getTiles(idArray);
}

LogicalArray NemoInterface::containsTiles(const IDArray &idArray) {
  return this->pImpl->containsTiles(idArray);
}

std::size_t NemoInterface::size() { return this->pImpl->size(); }

bool NemoInterface::empty() { return this->pImpl->empty(); }

ProgressArray NemoInterface::getProgress() {
  return this->pImpl->getProgress();
}

ProgressArray NemoInterface::getProgress(const IDArray &idArray) {
  return this->pImpl->getProgress(idArray);
}

void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }

void NemoInterface::requestProgress() {
  qCWarning(NemoInterfaceLog) << "requestProgress(): dummy.";
}

void NemoInterface::setTileData(const TileData &tileData) {
  this->pImpl->setTileData(tileData);
}

bool NemoInterface::hasTileData(const TileData &tileData) const {
  return this->pImpl->hasTileData(tileData);
}

NemoInterface::STATUS NemoInterface::status() const {
  return this->pImpl->status();
}

QString NemoInterface::statusString() const {
  return statusMap.at(this->pImpl->status());
}

QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }

QString NemoInterface::editorQml() {
  return QStringLiteral("NemoInterface.qml");
}

bool NemoInterface::running() { return this->pImpl->running(); }