NemoInterface.cpp

#include "NemoInterface.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 "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 setTilesENU(const SnakeTilesLocal &tilesENU);
  void setENUOrigin(const QGeoCoordinate &ENUOrigin);
  NemoInterface::NemoStatus status();
  QVector<int> progress();

private:
  bool doTopicServiceSetup();
  void loop();
  //!
  //! \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;
  mutable std::shared_timed_mutex progressMutex;
  QNemoHeartbeat heartbeat;
  TimePoint nextTimeout;
  mutable std::shared_timed_mutex heartbeatMutex;

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

NemoInterface::Impl::Impl(NemoInterface *p)
    : nextTimeout(TimePoint::max()), 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; }

void NemoInterface::Impl::stop() { this->running = false; }

void NemoInterface::Impl::setTilesENU(const SnakeTilesLocal &tilesENU) {
  UniqueLock lk(this->tilesENUMutex);
  this->tilesENU = tilesENU;
  lk.unlock();
  if (this->running && this->topicServiceSetupDone) {
    lk.lock();
    this->publishTilesENU();
  }
}

void NemoInterface::Impl::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
  UniqueLock lk(this->ENUOriginMutex);
  this->ENUOrigin = ENUOrigin;
  lk.unlock();
  if (this->running && this->topicServiceSetupDone) {
    lk.lock();
    this->publishENUOrigin();
  }
}

NemoInterface::NemoStatus NemoInterface::Impl::status() {
  SharedLock lk(this->heartbeatMutex);
  return NemoInterface::NemoStatus(heartbeat.status());
}

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

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

  // Publish 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());
    this->publishTilesENU();
  }

  // Publish snake origin.
  {
    SharedLock lk(this->ENUOriginMutex);

    this->pRosBridge->advertiseTopic(
        "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
    this->publishENUOrigin();
  }

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

          // 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");
        }
        lk1.unlock();
        lk2.unlock();
        lk3.unlock();

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

  // Subscribe /nemo/heartbeat.
  this->pRosBridge->subscribe(
      "/nemo/heartbeat",
      /* callback */ [this](JsonDocUPtr pDoc) {
        //        auto start = std::chrono::high_resolution_clock::now();
        UniqueLock lk(this->heartbeatMutex);

        auto &heartbeatMsg = this->heartbeat;
        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
          heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
          this->nextTimeout = TimePoint::max();
        } else {
          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;
    } 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;
  }

  // Check if heartbeat timeout occured.
  if (this->running && this->topicServiceSetupDone) {
    UniqueLock lk(this->heartbeatMutex);
    if (this->nextTimeout != TimePoint::max() &&
        this->nextTimeout < std::chrono::high_resolution_clock::now()) {
      this->heartbeat.setStatus(integral(NemoStatus::Timeout));
      lk.unlock();
      emit this->parent->statusChanged();
    }
  }
}

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::setTilesENU(const SnakeTilesLocal &tilesENU) {
  this->pImpl->setTilesENU(tilesENU);
}

void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
  this->pImpl->setENUOrigin(ENUOrigin);
}

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

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