CSWorker.cpp 3.96 KB
Newer Older
1 2 3 4 5 6
#include "CSWorker.h"
// std
#include <chrono>
// Qt
#include <QDebug>

7 8
RoutingWorker::RoutingWorker(QObject *parent)
    : QThread(parent), _calculating(false), _stop(false), _restart(false) {
9

10 11 12 13
  static std::once_flag flag;
  std::call_once(flag,
                 [] { qRegisterMetaType<PtrRoutingData>("PtrRoutingData"); });
}
14

15
RoutingWorker::~RoutingWorker() {
16 17 18 19 20 21 22 23
  this->_stop = true;
  Lock lk(this->_mutex);
  this->_restart = true;
  this->_cv.notify_one();
  lk.unlock();
  this->wait();
}

24
bool RoutingWorker::calculating() { return this->_calculating; }
25

26 27
void RoutingWorker::route(const snake::BoostPolygon &safeArea,
                          const RoutingWorker::Generator &generator) {
28 29 30
  // Sample input.
  Lock lk(this->_mutex);
  this->_safeArea = safeArea;
31
  this->_generator = generator;
32 33 34 35 36 37 38 39 40 41 42
  lk.unlock();

  if (!this->isRunning()) {
    this->start();
  } else {
    Lock lk(this->_mutex);
    this->_restart = true;
    this->_cv.notify_one();
  }
}

43 44
void RoutingWorker::run() {
  qWarning() << "RoutingWorker::run(): thread start.";
45 46
  while (!this->_stop) {
#ifdef DEBUG_CIRCULAR_SURVEY
47 48
    qWarning() << "RoutingWorker::run(): calculation "
                  "started.";
49
#endif
50
    // Copy input.
51
#ifdef SHOW_CIRCULAR_SURVEY_TIME
52
    auto start = std::chrono::high_resolution_clock::now();
53
#endif
54 55 56 57 58 59
    this->_calculating = true;
    emit calculatingChanged();
    Lock lk(this->_mutex);
    auto safeAreaENU = this->_safeArea;
    auto generator = this->_generator;
    lk.unlock();
60

61 62 63 64 65 66
    PtrRoutingData pRouteData(new RoutingData());
    auto &transectsENU = pRouteData->transects;
    // Generate transects.
    if (generator(transectsENU)) {
      // Check if generation was successful.
      if (transectsENU.size() == 0) {
67
#ifdef DEBUG_CIRCULAR_SURVEY
68 69
        qWarning() << "RoutingWorker::run(): "
                      "not able to generate transects.";
70
#endif
71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
      } else {
        // Prepare data for routing.
        auto &transectsInfo = pRouteData->transectsInfo;
        auto &route = pRouteData->route;
        const auto routingStart = std::chrono::high_resolution_clock::now();
        const auto maxRoutingTime = std::chrono::minutes(1);
        const auto routingEnd = routingStart + maxRoutingTime;
        const auto &restart = this->_restart;
        auto stopLambda = [&restart, routingEnd] {
          bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
          return restart || expired;
        };
        std::string errorString;
        // Route transects;
        bool success = snake::route(safeAreaENU, transectsENU, transectsInfo,
                                    route, stopLambda, errorString);
        // Check if routing was successful.
        if ((!success || route.size() < 3) && !this->_restart) {
89
#ifdef DEBUG_CIRCULAR_SURVEY
90 91
          qWarning() << "RoutingWorker::run(): "
                        "routing failed.";
92 93 94
#endif
        } else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
95
          qWarning() << "RoutingWorker::run(): "
96 97 98
                        "restart requested.";
#endif
        } else {
99 100
          // Notify main thread.
          emit result(pRouteData);
101
#ifdef DEBUG_CIRCULAR_SURVEY
102 103
          qWarning() << "RoutingWorker::run(): "
                        "concurrent update success.";
104 105 106
#endif
        }
      }
107
    } // end calculation
108 109
#ifdef DEBUG_CIRCULAR_SURVEY
    else {
110
      qWarning() << "RoutingWorker::run(): preconditions failed.";
111 112
    }
#endif
113 114 115 116 117 118 119 120 121
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    qWarning() << "RoutingWorker::run(): execution time: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::high_resolution_clock::now() - start)
                      .count()
               << " ms";
#endif
    this->_calculating = false;
    emit calculatingChanged();
122 123 124 125 126
    Lock lk2(this->_mutex);
    if (!this->_restart) {
      this->_cv.wait(lk2, [this] { return this->_restart.load(); });
    }
    this->_restart = false;
127 128
  } // main loop
  qWarning() << "RoutingWorker::run(): thread end.";
129
}