NemoInterface.cpp

#include "NemoInterface.h"
#include "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 "QNemoHeartbeat.h"
#include "QNemoProgress.h"
#include "Wima/Geometry/WimaMeasurementArea.h"
#include "Wima/Snake/SnakeTile.h"
#include "Wima/Snake/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"

QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")

#define EVENT_TIMER_INTERVAL 100 // ms
auto 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 {
  using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock>;

public:
  Impl(NemoInterface *p);

  void start();
  void stop();
  void setTileData(const TileData &tileData);
  bool hasTileData(const TileData &tileData) const;
  void setAutoPublish(bool ap);
  void setHoldProgress(bool hp);

  void publishTileData();

  NemoInterface::STATUS status();
  QVector<int> progress();
  bool running();

private:
  void doTopicServiceSetup();
  void loop();
  static STATUS heartbeatToStatus(
      const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb);
  //!
  //! \brief Publishes tilesENU
  //! \pre this->tilesENUMutex must be locked
  //!
  void publishTilesENU();
  //!
  //! \brief Publishes ENUOrigin
  //! \pre this->ENUOriginMutex must be locked
  //!
  void publishENUOrigin();
  bool setStatus(NemoInterface::STATUS s);

  // Data.
  SnakeTilesLocal tilesENU;
  mutable std::shared_timed_mutex tilesENUMutex;
  QGeoCoordinate ENUOrigin;
  mutable std::shared_timed_mutex ENUOriginMutex;
  QNemoProgress qProgress;
  mutable std::shared_timed_mutex progressMutex;
  TimePoint nextTimeout;
  mutable std::shared_timed_mutex timeoutMutex;
  std::atomic<NemoInterface::STATUS> status_;

  // Not protected data.
  TileData tileData;

  // Internals
  std::atomic_bool running_;
  std::atomic_bool topicServiceSetupDone;
  ROSBridgePtr pRosBridge;
  QTimer loopTimer;
  NemoInterface *parent;
};

using StatusMap = std::map<NemoInterface::STATUS, QString>;
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())) {
      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->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();
}

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.get(0);
    const auto *tile = qobject_cast<const SnakeTile *>(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.get(i);
            tile = qobject_cast<const SnakeTile *>(obj);
            if (tile != nullptr) {
              SnakeTileLocal tileENU;
              snake::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 status_.load(); }

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

bool NemoInterface::Impl::running() { return this->running_.load(); }

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()->showMessage("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->setStatus(STATUS::INVALID_HEARTBEAT);
        } else {
          this->setStatus(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->setStatus(STATUS::WEBSOCKET_DETECTED);
    } else if (this->pRosBridge->isRunning() &&
               !this->pRosBridge->connected() && this->topicServiceSetupDone) {
      this->pRosBridge->reset();
      this->pRosBridge->start();
      this->topicServiceSetupDone = false;

      this->setStatus(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->setStatus(STATUS::WEBSOCKET_DETECTED);
      } else {
        this->setStatus(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::setStatus(NemoInterface::STATUS s) {
  if (s != this->status_) {
    this->status_ = s;
    emit this->parent->statusChanged();
    return true;
  } else {
    return false;
  }
}

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

NemoInterface::~NemoInterface() {}

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

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

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

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

NemoInterface::STATUS NemoInterface::statusEnum() 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(); }