SnakeDataManager.cc 13.5 KB
Newer Older
1 2 3 4 5 6 7 8
#include "SnakeDataManager.h"

#include <QGeoCoordinate>
#include <QMutexLocker>

#include "QGCApplication.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
9 10
#include "SettingsManager.h"
#include "WimaSettings.h"
11 12 13

#include <memory>
#include <mutex>
14
#include <shared_mutex>
15 16 17

#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
18
#include "snake.h"
19 20 21 22 23 24

using QVariantList = QList<QVariant>;

using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;

25
class SnakeDataManager::SnakeImpl {
26
public:
Valentin Platzgummer's avatar
Valentin Platzgummer committed
27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71
  struct Input {

    Input()
        : tileWidth(5 * si::meter), tileHeight(5 * si::meter),
          minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter),
          minTransectLength(1 * si::meter), scenarioChanged(true),
          routeParametersChanged(true) {}

    QList<QGeoCoordinate> mArea;
    QGeoCoordinate ENUOrigin;
    QList<QGeoCoordinate> sArea;
    QList<QGeoCoordinate> corridor;
    Length tileWidth;
    Length tileHeight;
    Area minTileArea;
    std::atomic_bool scenarioChanged;

    Length lineDistance;
    Length minTransectLength;
    std::atomic_bool routeParametersChanged;

    mutable std::shared_timed_mutex mutex;
  };
  struct Output {
    SnakeTiles tiles;
    QmlObjectListModel tilesQml;
    QVariantList tileCenterPoints;
    SnakeTilesLocal tilesENU;
    QVector<QPointF> tileCenterPointsENU;

    QVector<QGeoCoordinate> waypoints;
    QVector<QGeoCoordinate> arrivalPath;
    QVector<QGeoCoordinate> returnPath;
    QVector<QPointF> waypointsENU;
    QVector<QPointF> arrivalPathENU;
    QVector<QPointF> returnPathENU;

    QString errorMessage;

    std::atomic_bool calcInProgress;

    mutable std::shared_timed_mutex mutex;
  };

  SnakeImpl(SnakeDataManager *p);
72 73 74 75

  bool precondition() const;
  bool doTopicServiceSetup();

Valentin Platzgummer's avatar
Valentin Platzgummer committed
76
  // Internal data.
77 78
  snake::Scenario scenario;
  SnakeDataManager *parent;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
79 80 81 82

  Input input;
  // Output
  Output output;
83 84
};

Valentin Platzgummer's avatar
Valentin Platzgummer committed
85 86
SnakeDataManager::SnakeImpl::SnakeImpl(SnakeDataManager *p)
    : rosBridgeEnabeled(false), topicServiceSetupDone(false), parent(p) {}
87

88
bool SnakeDataManager::SnakeImpl::precondition() const { return true; }
89

90 91 92 93 94
template <class Callable> class CommandRAII {
public:
  CommandRAII(Callable &fun) : _fun(fun) {}

  ~CommandRAII() { _fun(); }
95

96 97 98 99 100 101 102 103 104
private:
  Callable _fun;
};

SnakeDataManager::SnakeDataManager(QObject *parent)
    : QThread(parent), pImpl(std::make_unique<SnakeImpl>(this))

{}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
105
// SnakeDataManager::~SnakeDataManager() {}
106 107 108

void SnakeDataManager::setMeasurementArea(
    const QList<QGeoCoordinate> &measurementArea) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
109 110 111 112
  this.input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->inptu.mArea = measurementArea;
  for (auto &vertex : this->pImpl->input.mArea) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
113 114
    vertex.setAltitude(0);
  }
115 116
}

117 118
void SnakeDataManager::setServiceArea(
    const QList<QGeoCoordinate> &serviceArea) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
119 120 121 122
  this.input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.sArea = serviceArea;
  for (auto &vertex : this->pImpl->input.sArea) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
123 124
    vertex.setAltitude(0);
  }
125 126
}

127
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
128 129 130 131
  this.input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.corridor = corridor;
  for (auto &vertex : this->pImpl->input.corridor) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
132 133
    vertex.setAltitude(0);
  }
134 135
}

136
const QmlObjectListModel *SnakeDataManager::tiles() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
137 138
  SharedLock lk(this->pImpl->output.mutex);
  return &this->pImpl->output.tilesQml;
139 140
}

141
QVariantList SnakeDataManager::tileCenterPoints() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
142 143
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tileCenterPoints;
144 145
}

146
QNemoProgress SnakeDataManager::nemoProgress() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
147 148
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.qProgress;
149 150
}

151
int SnakeDataManager::nemoStatus() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
152 153
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.heartbeat.status();
154 155
}

156
bool SnakeDataManager::calcInProgress() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
157
  return this->pImpl->output.calcInProgress.load();
158 159
}

160
QString SnakeDataManager::errorMessage() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
161 162
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.errorMessage;
163 164
}

165
bool SnakeDataManager::success() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
166 167
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.errorMessage.isEmpty() ? true : false;
168 169
}

170
QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
171 172
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.waypoints;
173 174
}

175
QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
176 177
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.arrivalPath;
178 179
}

180
QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
181 182
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.returnPath;
183 184
}

185 186 187
Length SnakeDataManager::lineDistance() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->lineDistance;
188 189
}

190 191
void SnakeDataManager::setLineDistance(Length lineDistance) {
  UniqueLock lk(this->pImpl->mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
192
  routeParametersChanged = true;
193
  this->pImpl->lineDistance = lineDistance;
194 195
}

196 197 198
Area SnakeDataManager::minTileArea() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->scenario.minTileArea();
199 200
}

201
void SnakeDataManager::setMinTileArea(Area minTileArea) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
202
  scenarioChanged = true;
203 204
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->scenario.setMinTileArea(minTileArea);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
205 206
}

207 208 209 210
Length SnakeDataManager::tileHeight() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->scenario.tileHeight();
}
211

212
void SnakeDataManager::setTileHeight(Length tileHeight) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
213
  scenarioChanged = true;
214 215 216
  UniqueLock lk(this->pImpl->mutex);
  this->pImpl->scenario.setTileHeight(tileHeight);
}
217

218 219 220 221
Length SnakeDataManager::tileWidth() const {
  SharedLock lk(this->pImpl->mutex);
  return this->pImpl->scenario.tileWidth();
}
222

223 224
void SnakeDataManager::setTileWidth(Length tileWidth) {
  UniqueLock lk(this->pImpl->mutex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
225
  scenarioChanged = true;
226 227
  this->pImpl->scenario.setTileWidth(tileWidth);
}
228

229 230 231
void SnakeDataManager::enableRosBridge() {
  this->pImpl->rosBridgeEnabeled = true;
}
232

233 234 235
void SnakeDataManager::disableRosBride() {
  this->pImpl->rosBridgeEnabeled = false;
}
236

237
void SnakeDataManager::run() {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
238 239 240 241
#ifndef NDEBUG
  auto startTime = std::chrono::high_resolution_clock::now();
#endif

242 243
  this->pImpl->calcInProgress.store(true);
  emit calcInProgressChanged(this->pImpl->calcInProgress.load());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
244 245 246 247 248 249 250 251
  auto onExit = [this, &startTime] {
#ifndef NDEBUG
    qDebug() << "SnakeDataManager::run() execution time: "
             << std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::high_resolution_clock::now() - startTime)
                    .count()
             << " ms";
#endif
252 253 254 255 256
    this->pImpl->calcInProgress.store(false);
    emit calcInProgressChanged(this->pImpl->calcInProgress.load());
  };
  CommandRAII<decltype(onExit)> onExitRAII(onExit);

Valentin Platzgummer's avatar
Valentin Platzgummer committed
257 258
  // Check preconditions.
  SharedLock sLock(this->pImpl->mutex);
259 260 261 262 263 264 265 266 267 268 269 270 271

  if (!this->pImpl->precondition())
    return;

  if (this->pImpl->mArea.size() < 3) {
    this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
    return;
  }
  if (this->pImpl->sArea.size() < 3) {
    this->pImpl->errorMessage = "Service area invalid: size < 3.";
    return;
  }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286
  // Update Scenario if necessary
  if (this->pImpl->scenarioChanged) {
    // Set Origin
    this->pImpl->ENUOrigin = this->pImpl->mArea.front();
    auto &origin = this->pImpl->ENUOrigin;

    // Update measurement area.
    auto &mAreaEnu = this->pImpl->scenario.measurementArea();
    auto &mArea = this->pImpl->mArea;
    mAreaEnu.clear();
    for (auto geoVertex : mArea) {
      snake::BoostPoint p;
      snake::toENU(origin, geoVertex, p);
      mAreaEnu.outer().push_back(p);
    }
287

Valentin Platzgummer's avatar
Valentin Platzgummer committed
288 289 290 291 292 293 294 295 296
    // Update service area.
    auto &sAreaEnu = this->pImpl->scenario.serviceArea();
    auto &sArea = this->pImpl->sArea;
    sAreaEnu.clear();
    for (auto geoVertex : sArea) {
      snake::BoostPoint p;
      snake::toENU(origin, geoVertex, p);
      sAreaEnu.outer().push_back(p);
    }
297

Valentin Platzgummer's avatar
Valentin Platzgummer committed
298 299 300 301 302 303 304 305 306 307 308 309 310 311
    // Update corridor.
    auto &corridorEnu = this->pImpl->scenario.corridor();
    auto &corridor = this->pImpl->corridor;
    corridor.clear();
    for (auto geoVertex : corridor) {
      snake::BoostPoint p;
      snake::toENU(origin, geoVertex, p);
      corridorEnu.outer().push_back(p);
    }

    if (!this->pImpl->scenario.update()) {
      this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
      return;
    }
312
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354
  sLock.unlock;

  bool storeProgress = false;
  bool storeRoute = false;
  if (this->pImpl->scenarioChanged || this->pImpl->routeParametersChanged) {
    storeRoute = true;
    // Sample data
    SharedLock lk(this->pImpl->mutex);
    size_t nTiles = this->pImpl->tiles.polygons().size();
    if (this->pImpl->progress.size() != nTiles) {
    }
    const auto scenario = this->pImpl->scenario;
    std::vector<int> progress;
    size_t nTiles = this->pImpl->tiles.polygons().size();
    if (this->pImpl->progress.size() != nTiles) {
      progress.reserve(nTiles);
      for (size_t i = 0; i < nTiles; ++i) {
        progress.push_back(0);
      }
      storeProgress = true;
    } else {
      progress = this->pImpl->progress;
    }
    const auto lineDistance = this->pImpl->lineDistance;
    const auto minTransectLength = this->pImpl->minTransectLength;
    lk.unlock();

    // Create transects.
    std::string errorString;
    snake::Angle alpha(-scenario.mAreaBoundingBox().angle * degree::degree);
    snake::flight_plan::Transects transects;
    transects.push_back(
        bg::model::linestring<snake::BoostPoint>{scenario.homePositon()});

    bool value = snake::flight_plan::transectsFromScenario(
        lineDistance, minTransectLength, alpha, scenario, progress, transects,
        errorString);
    if (!value) {
      UniqueLock lk(this->pImpl->mutex);
      this->pImpl->errorMessage = errorString.c_str();
      storeRoute = false;
    }
355

Valentin Platzgummer's avatar
Valentin Platzgummer committed
356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406
    // Route transects
    if (transects.size() > 1) {
      snake::flight_plan::Transects transectsRouted;
      snake::flight_plan::Route route;
      value = snake::flight_plan::route(scenario.joinedArea(), transects,
                                        transectsRouted, route, errorString);
      if (!value) {
        this->pImpl->errorMessage = errorString.c_str();
        return;
      }

      // Store arrival path.
      const auto &firstWaypoint = transectsRouted.front().front();
      long startIdx = 0;
      for (long i = 0; i < long(route.size()); ++i) {
        const auto &boostVertex = route[i];
        if (boostVertex == firstWaypoint) {
          startIdx = i;
          break;
        }
        QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
        QGeoCoordinate geoVertex;
        snake::fromENU(origin, boostVertex, geoVertex);
        this->pImpl->arrivalPathENU.push_back(enuVertex);
        this->pImpl->arrivalPath.push_back(geoVertex);
      }
      // Store return path.
      long endIdx = 0;
      const auto &lastWaypoint = transectsRouted.back().back();
      for (long i = route.size() - 1; i >= 0; --i) {
        const auto &boostVertex = route[i];
        if (boostVertex == lastWaypoint) {
          endIdx = i;
          break;
        }
        QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
        QGeoCoordinate geoVertex;
        snake::fromENU(origin, boostVertex, geoVertex);
        this->pImpl->returnPathENU.push_back(enuVertex);
        this->pImpl->returnPath.push_back(geoVertex);
      }
      // Store waypoints.
      for (long i = startIdx; i <= endIdx; ++i) {
        const auto &boostVertex = route[i];
        QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
        QGeoCoordinate geoVertex;
        snake::fromENU(origin, boostVertex, geoVertex);
        this->pImpl->waypointsENU.push_back(enuVertex);
        this->pImpl->waypoints.push_back(geoVertex);
      }
    }
407 408
  }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
409 410 411 412 413 414 415 416 417 418
  UniqueLock uLock(this->pImpl->mutex);
  // Store scenario.
  if (this->pImpl->scenarioChanged) {
    // Clear some output data.
    this->pImpl->tiles.polygons().clear();
    this->pImpl->tilesQml.clearAndDeleteContents();
    this->pImpl->tileCenterPoints.clear();
    this->pImpl->tilesENU.polygons().clear();
    this->pImpl->tileCenterPointsENU.clear();
    // Convert and store scenario data.
419 420 421 422 423 424
    const auto &tiles = this->pImpl->scenario.tiles();
    const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
    for (unsigned int i = 0; i < tiles.size(); ++i) {
      const auto &tile = tiles[i];
      SnakeTile geoTile;
      SnakeTileLocal enuTile;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
425
      for (size_t i = 0; i < tile.outer().size() - 1; ++i) {
426 427 428 429 430 431 432 433 434 435 436 437 438
        auto &p = tile.outer()[i];
        QPointF enuVertex(p.get<0>(), p.get<1>());
        QGeoCoordinate geoVertex;
        snake::fromENU(origin, p, geoVertex);
        enuTile.polygon().points().push_back(enuVertex);
        geoTile.push_back(geoVertex);
      }
      const auto &boostPoint = centerPoints[i];
      QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
      QGeoCoordinate geoVertex;
      snake::fromENU(origin, boostPoint, geoVertex);
      geoTile.setCenter(geoVertex);
      this->pImpl->tiles.polygons().push_back(geoTile);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
439 440 441
      auto *geoTileCopy = new SnakeTile(geoTile);
      geoTileCopy->moveToThread(this->pImpl->tilesQml.thread());
      this->pImpl->tilesQml.append(geoTileCopy);
442 443 444 445 446
      this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
      this->pImpl->tilesENU.polygons().push_back(enuTile);
      this->pImpl->tileCenterPointsENU.push_back(enuVertex);
    }
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
447 448
  // Store route etc.
  if (this->pImpl->scenarioChanged || this->pImpl->routeParametersChanged) {
449
  }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
450 451 452
  uLock.unlock;
  this->pImpl->scenarioChanged = false;
  this->pImpl->routeParametersChanged = false;
453
}