NemoInterface.cpp

#include "NemoInterface.h"
#include "SnakeTilesLocal.h"

#include "QGCApplication.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"

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

  bool lockProgress();
  void publishTileData();

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

private:
  bool 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();

  // Data.
  SnakeTilesLocal tilesENU;
  mutable std::shared_timed_mutex tilesENUMutex;
  QGeoCoordinate ENUOrigin;
  mutable std::shared_timed_mutex ENUOriginMutex;
  QNemoProgress qProgress;
  QNemoProgress qProgressHolded;
  mutable std::shared_timed_mutex progressMutex;
  NemoInterface::STATUS status_;
  TimePoint nextTimeout;
  mutable std::shared_timed_mutex statusMutex;

  // Not protected data.
  TileData tileData;

  // Internals
  std::atomic_bool running_;
  std::atomic_bool lockProgress_;
  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)
    : status_(STATUS::NOT_CONNECTED), nextTimeout(TimePoint::max()),
      running_(false), lockProgress_(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) {
        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 {
            qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
            break;
          }
        }
      } else {
        qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
      }
    }
  }
}

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

void NemoInterface::Impl::setHoldProgress(bool hp) {
  if (this->lockProgress_ != hp) {
    this->lockProgress_ = hp;
    emit this->parent->lockProgressChanged();

    if (!this->lockProgress_) {
      UniqueLock lk(this->progressMutex);
      if (this->qProgress != this->qProgressHolded) {
        this->qProgressHolded = this->qProgress;
        lk.unlock();
        emit this->parent->progressChanged();
      }
    }
  }
}

bool NemoInterface::Impl::lockProgress() { return this->lockProgress_.load(); }

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() {
  SharedLock lk(this->statusMutex);
  return status_;
}

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

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

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

  // snake tiles.
  {
    SharedLock lk(this->tilesENUMutex);

    if (this->tilesENU.polygons().size() == 0)
      return false;
    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 hold = this->lockProgress_.load();
        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.");
        } else if (!hold) {
          this->qProgressHolded = this->qProgress;
        }
        lk1.unlock();
        lk2.unlock();
        lk3.unlock();

        if (!hold) {
          emit this->parent->progressChanged();
        }
      });

  // 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;
        UniqueLock lk(this->statusMutex);
        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
          status_ = STATUS::INVALID_HEARTBEAT;
        } else {
          status_ = heartbeatToStatus(heartbeatMsg);
        }
        if (status_ == STATUS::INVALID_HEARTBEAT) {
          this->nextTimeout = TimePoint::max();
        } else if (status_ == STATUS::HEARTBEAT_DETECTED) {
          this->nextTimeout =
              std::chrono::high_resolution_clock::now() + timeoutInterval;
        }
        lk.unlock();

        emit this->parent->statusChanged();
        //        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);
        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->tilesENUMutex);

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

void NemoInterface::Impl::loop() {
  // Check ROS Bridge status and do setup if necessary.
  if (this->running_) {
    if (!this->pRosBridge->isRunning()) {
      this->pRosBridge->start();
    } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
               !this->topicServiceSetupDone) {
      if (this->doTopicServiceSetup()) {
        this->topicServiceSetupDone = true;
        UniqueLock lk(this->statusMutex);
        this->status_ = STATUS::WEBSOCKET_DETECTED;
        lk.unlock();
        emit this->parent->statusChanged();
      }
    } else if (this->pRosBridge->isRunning() &&
               !this->pRosBridge->connected() && this->topicServiceSetupDone) {
      this->pRosBridge->reset();
      this->pRosBridge->start();
      this->topicServiceSetupDone = false;

      UniqueLock lk(this->statusMutex);
      this->status_ = STATUS::TIMEOUT;
      lk.unlock();
      emit this->parent->statusChanged();
    }
  } else if (this->pRosBridge->isRunning()) {
    this->pRosBridge->reset();
    this->topicServiceSetupDone = false;
  }

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

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

void NemoInterface::Impl::publishENUOrigin() {
  using namespace ros_bridge::messages;
  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");
}

// ===============================================================
// 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() {
  qWarning() << "NemoInterface::requestProgress(): dummy.";
}

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

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

void NemoInterface::setHoldProgress(bool hp) {
  this->pImpl->setHoldProgress(hp);
}

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

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