Skip to content
GitLab
Commit cf53d945 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

SnakeDataManager mod

parent f3a5cb44
Hide whitespace changes
Inline Side-by-side
qgroundcontrol.pro
View file @ cf53d945
......@@ -441,7 +441,7 @@ HEADERS += \
src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/Snake/SnakeW.h \
src/Wima/Snake/SnakeDataManager.h \
src/Wima/WaypointManager/AreaInterface.h \
src/Wima/WaypointManager/DefaultManager.h \
src/Wima/WaypointManager/GenericWaypointManager.h \
......
src/Snake/snake.cpp
View file @ cf53d945
......@@ -1041,7 +1041,6 @@ bool flight_plan::route(const BoostPolygon &area,
route_idx.push_back(manager.IndexToNode(index).value());
}
// Helper Lambda.
auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
std::vector<BoostPoint> &path){
......@@ -1054,17 +1053,31 @@ bool flight_plan::route(const BoostPolygon &area,
size_t idx0 = route_idx[i];
size_t idx1 = route_idx[i+1];
const auto &info1 = transectInfoList[idx0];
const auto &info2 = transectInfoList[idx0];
if ( info.front ){
const auto &t = transects[info.index];
for ( auto it = t.begin(); it < t.end()-1; ++it){
route.push_back(*it);
const auto &info2 = transectInfoList[idx1];
if (info1.index == info2.index) { // same transect?
if ( !info1.front ) { // transect reversal needed?
BoostLineString reversedTransect;
const auto &t = transects[info1.index];
for ( auto it = t.end()-1; it >= t.begin(); --it){
reversedTransect.push_back(*it);
}
transectsRouted.push_back(reversedTransect);
for (auto it = reversedTransect.begin(); it < reversedTransect.end()-1; ++it){
route.push_back(*it);
}
} else {
const auto &t = transects[info1.index];
for ( auto it = t.begin(); it < t.end()-1; ++it){
route.push_back(*it);
}
transectsRouted.push_back(t);
}
transectsRouted.push_back(t);
} else {
std::vector<size_t> idxList;
shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
idxList.pop_back();
if ( i != route_idx.size()-2){
idxList.pop_back();
}
idx2Vertex(idxList, route);
}
}
......
src/Snake/snake_typedefs.h
View file @ cf53d945
......@@ -22,8 +22,21 @@ typedef bg::model::linestring<BoostPoint> BoostLineString;
typedef std::vector<std::vector<int64_t>> Int64Matrix;
typedef bg::model::box<BoostPoint> BoostBox;
typedef bu::quantity<bu::si::length, double> Length;
typedef bu::quantity<bu::si::area, double> Area;
typedef bu::quantity<bu::si::plane_angle, double> Angle;
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
}
namespace boost{
namespace geometry{
namespace model {
bool operator==(snake::BoostPoint p1, snake::BoostPoint p2){
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2){
return !(p1 == p2);
}
} // namespace model
} // namespace geometry
} // namespace boost
src/Wima/Snake/SnakeDataManager.cc
View file @ cf53d945
......@@ -17,71 +17,74 @@
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "comm/ros_bridge/include/ros_bridge.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#define EVENT_TIMER_INTERVAL 500 // ms
#define TIMEOUT 3000 // ms
using QVariantList = QList<QVariant>;
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
class SnakeImpl : public QObject{
Q_OBJECT
public:
SnakeImpl() :
lineDistance(1*si::meter)
, minTransectLength(1*si::meter)
, calcInProgress(false)
, topicServiceSetupDone(false)
{
// 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 {
qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
this->pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090"));
}
};
setConnectionString();
connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
}
SnakeImpl(SnakeDataManager* p);
bool precondition() const;
void resetWaypointData();
bool doTopicServiceSetup();
// Private data.
ROSBridgePtr pRosBridge;
bool topicServiceSetupDone;
mutable std::shared_timed_mutex mutex;
// Input
snake::Scenario scenario;
Length lineDistance;
Length minTransectLength;
ROSBridgePtr pRosBridge;
bool rosBridgeEnabeled;
bool topicServiceSetupDone;
QTimer eventTimer;
QTimer timeout;
mutable std::shared_timed_mutex mutex;
// Scenario
snake::Scenario scenario;
Length lineDistance;
Length minTransectLength;
QList<QGeoCoordinate> mArea;
QList<QGeoCoordinate> sArea;
QList<QGeoCoordinate> corridor;
QGeoCoordinate ENUOrigin;
SnakeTiles tiles;
QVariantList tileCenterPoints;
SnakeTilesLocal tilesENU;
QVector<QPointF> tileCenterPointsENU;
// Output
std::atomic_bool calcInProgress;
QNemoProgress qProgress;
std::vector<int> progress;
QNemoHeartbeat heartbeat;
// Waypoints
QVector<QGeoCoordinate> waypoints;
QVector<QGeoCoordinate> arrivalPath;
QVector<QGeoCoordinate> returnPath;
QGeoCoordinate ENUorigin;
QVector<QPointF> waypointsENU;
QVector<QPointF> arrivalPathENU;
QVector<QPointF> returnPathENU;
SnakeTiles tiles;
QVariantList tileCenterPoints;
SnakeTilesLocal tilesENU;
QVector<QPointF> tileCenterPointsENU;
QString errorMessage;
// Other
std::atomic_bool calcInProgress;
QNemoProgress qProgress;
std::vector<int> progress;
QNemoHeartbeat heartbeat;
SnakeDataManager *parent;
};
template<class AtomicType>
......@@ -107,7 +110,7 @@ private:
SnakeDataManager::SnakeDataManager(QObject *parent)
: QThread(parent)
, pImpl(std::make_unique<SnakeImpl>())
, pImpl(std::make_unique<SnakeImpl>(this))
{
}
......@@ -135,16 +138,16 @@ void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
this->pImpl->corridor = corridor;
}
QNemoProgress SnakeDataManager::progress()
QNemoProgress SnakeDataManager::nemoProgress()
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->qProgress;
}
QNemoHeartbeat SnakeDataManager::heartbeat()
int SnakeDataManager::nemoStatus()
{
SharedLock lk(this->pImpl->mutex);
return this->pImpl->heartbeat;
return this->pImpl->heartbeat.status();
}
bool SnakeDataManager::calcInProgress()
......@@ -200,7 +203,17 @@ void SnakeDataManager::setTileWidth(Length tileWidth)
this->pImpl->scenario.setTileWidth(tileWidth);
}
bool SnakeDataManager::precondition() const
void SnakeDataManager::enableRosBridge()
{
this->pImpl->rosBridgeEnabeled = true;
}
void SnakeDataManager::disableRosBride()
{
this->pImpl->rosBridgeEnabeled = false;
}
bool SnakeImpl::precondition() const
{
return true;
}
......@@ -208,20 +221,20 @@ bool SnakeDataManager::precondition() const
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeDataManager::resetWaypointData()
void SnakeImpl::resetWaypointData()
{
this->pImpl->waypoints.clear();
this->pImpl->arrivalPath.clear();
this->pImpl->returnPath.clear();
this->pImpl->ENUorigin = QGeoCoordinate(0.0,0.0,0.0);
this->pImpl->waypointsENU.clear();
this->pImpl->arrivalPathENU.clear();
this->pImpl->returnPathENU.clear();
this->pImpl->tiles.polygons().clear();
this->pImpl->tileCenterPoints.clear();
this->pImpl->tilesENU.polygons().clear();
this->pImpl->tileCenterPointsENU.clear();
this->pImpl->errorMessage.clear();
this->waypoints.clear();
this->arrivalPath.clear();
this->returnPath.clear();
this->ENUOrigin = QGeoCoordinate(0.0,0.0,0.0);
this->waypointsENU.clear();
this->arrivalPathENU.clear();
this->returnPathENU.clear();
this->tiles.polygons().clear();
this->tileCenterPoints.clear();
this->tilesENU.polygons().clear();
this->tileCenterPointsENU.clear();
this->errorMessage.clear();
}
void SnakeDataManager::run()
......@@ -230,23 +243,23 @@ void SnakeDataManager::run()
ToggleRAII<std::atomic_bool> tr(this->pImpl->calcInProgress);
UniqueLock lk(this->pImpl->mutex);
resetWaypointData();
this->pImpl->resetWaypointData();
if ( !precondition() )
if ( !this->pImpl->precondition() )
return;
if ( this->pImpl->mArea.size() < 3) {
pImpl->errorMessage = "Measurement area invalid: size < 3.";
this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
return;
}
if ( this->pImpl->sArea.size() < 3) {
pImpl->errorMessage = "Service area invalid: size < 3.";
this->pImpl->errorMessage = "Service area invalid: size < 3.";
return;
}
this->pImpl->ENUorigin = this->pImpl->mArea.front();
auto &origin = this->pImpl->ENUorigin;
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;
......@@ -278,17 +291,19 @@ void SnakeDataManager::run()
}
if ( !this->pImpl->scenario.update() ){
pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
return;
}
// Asynchronously update flightplan.
qWarning() << "route calculation missing";
std::string errorString;
auto future = std::async([this, &errorString]{
auto alpha = -this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree;
auto future = std::async([this, &errorString, &origin]{
snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree);
snake::flight_plan::Transects transects;
transects.push_back(bg::model::linestring{this->pImpl->scenario.homePositon()});
transects.push_back(bg::model::linestring<snake::BoostPoint>{
this->pImpl->scenario.homePositon()
});
bool value = snake::flight_plan::transectsFromScenario(this->pImpl->lineDistance,
this->pImpl->minTransectLength,
alpha,
......@@ -308,29 +323,44 @@ void SnakeDataManager::run()
if ( !value )
return value;
// Store waypoints.
for (auto boostVertex : _pFlightplan->waypoints()){
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < 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);
pImpl->waypointsENU.push_back(enuVertex);
pImpl->waypoints.push_back(geoVertex);
this->pImpl->arrivalPathENU.push_back(enuVertex);
this->pImpl->arrivalPath.push_back(geoVertex);
}
// Store arrival path.
for (auto boostVertex : _pFlightplan->arrivalPath()){
// 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);
pImpl->arrivalPathENU.push_back(enuVertex);
pImpl->arrivalPath.push_back(geoVertex);
this->pImpl->returnPathENU.push_back(enuVertex);
this->pImpl->returnPath.push_back(geoVertex);
}
// Store return path.
for (auto boostVertex : _pFlightplan->returnPath()){
// 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);
pImpl->returnPathENU.push_back(enuVertex);
pImpl->returnPath.push_back(geoVertex);
this->pImpl->waypointsENU.push_back(enuVertex);
this->pImpl->waypoints.push_back(geoVertex);
}
return true;
......@@ -359,10 +389,10 @@ void SnakeDataManager::run()
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostPoint, geoVertex);
geoTile.setCenter(geoVertex);
pImpl->tiles.polygons().push_back(geoTile);
pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
pImpl->tilesENU.polygons().push_back(enuTile);
pImpl->tileCenterPointsENU.push_back(enuVertex);
this->pImpl->tiles.polygons().push_back(geoTile);
this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
this->pImpl->tilesENU.polygons().push_back(enuTile);
this->pImpl->tileCenterPointsENU.push_back(enuVertex);
}
}
......@@ -370,13 +400,187 @@ void SnakeDataManager::run()
// Trying to generate flight plan.
if ( !future.get() ){
// error
for (auto c : _pFlightplan->errorString){
pImpl->errorMessage.push_back(QChar(c));
}
this->pImpl->errorMessage = errorString.c_str();
} else {
// Success!!!
}
}
bool SnakeImpl::doTopicServiceSetup()
{
using namespace ros_bridge::messages;
UniqueLock lk(this->mutex);
if ( this->tilesENU.polygons().size() == 0)
return false;
// Publish snake origin.
this->pRosBridge->advertiseTopic("/snake/origin",
geographic_msgs::geo_point::messageType().c_str());
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.
this->pRosBridge->advertiseTopic("/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
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");
// Subscribe nemo progress.
this->pRosBridge->subscribe("/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc){
UniqueLock lk(this->mutex);
int requiredSize = this->tilesENU.polygons().size();
auto& progressMsg = this->qProgress;
if ( !nemo_msgs::progress::fromJson(*pDoc, progressMsg)
|| progressMsg.progress().size() != requiredSize ) { // Some error occured.
// Set progress to default.
progressMsg.progress().fill(0, requiredSize);
// 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");
}
this->progress.clear();
for (const auto&p : progressMsg.progress()){
this->progress.push_back(p);
}
lk.unlock();
emit this->parent->nemoProgressChanged();
});
// Subscribe /nemo/heartbeat.
this->pRosBridge->subscribe("/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc){
UniqueLock lk(this->mutex);
if ( this->timeout.isActive() ) {
this->timeout.stop();
}
auto &heartbeatMsg = this->heartbeat;
if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg) ) {
heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
}
this->timeout.singleShot(TIMEOUT, [this]{
UniqueLock lk(this->mutex);
this->heartbeat.setStatus(integral(NemoStatus::Timeout));
emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
});
emit this->parent->nemoStatusChanged(heartbeatMsg.status());
});
// Advertise /snake/get_origin.
this->pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr
{
using namespace ros_bridge::messages;
SharedLock lk(this->mutex);
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->mutex);
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;
}
SnakeImpl::SnakeImpl(SnakeDataManager *p) :
rosBridgeEnabeled(false)
, topicServiceSetupDone(false)
, lineDistance(1*si::meter)
, minTransectLength(1*si::meter)
, calcInProgress(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->eventTimer, &QTimer::timeout, [this]{
if ( this->rosBridgeEnabeled ) {
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;
}
});
this->eventTimer.start(EVENT_TIMER_INTERVAL);
}
src/Wima/Snake/SnakeDataManager.h
View file @ cf53d945
......@@ -8,7 +8,7 @@
#include "QNemoProgress.h"
#include "QNemoHeartbeat.h"
#include <boost/units/systems/si.hpp>>
#include <boost/units/systems/si.hpp>
using namespace boost::units;
......@@ -17,6 +17,13 @@ using Area = quantity<si::area>;
class SnakeImpl;
enum class NemoStatus{
NotConnected = 0,
Connected = 1,
Timeout = -1,
InvalidHeartbeat = -2
};
class SnakeDataManager : public QThread{
Q_OBJECT
......@@ -31,8 +38,8 @@ public:
void setServiceArea (const QList<QGeoCoordinate> &serviceArea);
void setCorridor (const QList<QGeoCoordinate> &corridor);
QNemoProgress progress();
QNemoHeartbeat heartbeat();
QNemoProgress nemoProgress();
int nemoStatus();
bool calcInProgress();
Length lineDistance() const;
......@@ -50,14 +57,17 @@ public:
Length tileWidth() const;
void setTileWidth(Length tileWidth);
void enableRosBridge();
void disableRosBride();
signals:
void nemoProgressChanged();
void nemoStatusChanged(int status);
void calcInProgressChanged(bool inProgress);
protected:
void run() override;
private:
bool precondition() const;
void resetWaypointData();
SnakeImplPtr pImpl;
};
......
src/Wima/WimaController.cc
View file @ cf53d945
......@@ -2,14 +2,6 @@
#include "utilities.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
......@@ -45,10 +37,10 @@ const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLengt
WimaController::StatusMap WimaController::_nemoStatusMap{
std::make_pair<int, QString>(0, "No Heartbeat"),
std::make_pair<int, QString>(1, "Connected"),
std::make_pair<int, QString>(-1, "Timeout")};
std::make_pair<int, QString>(-1, "Timeout"),
std::make_pair<int, QString>(-2, "Error")};
using namespace snake;
using namespace snake_geometry;
WimaController::WimaController(QObject *parent)
: QObject (parent)
......@@ -125,11 +117,19 @@ WimaController::WimaController(QObject *parent)
// Snake Worker Thread.
connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults);
connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit);
connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeDataManager::quit);
// Snake.
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
_initStartSnakeWorker();
auto configRosBridge = [this]{
if ( this->_enableSnake.rawValue().toBool() ){
this->_snakeDataManager.enableRosBridge();
} else {
this->_snakeDataManager.disableRosBride();
}
};
connect(&_enableSnake, &Fact::rawValueChanged, configRosBridge);
configRosBridge();
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
_switchSnakeManager(_enableSnake.rawValue());
}
......@@ -202,8 +202,9 @@ Fact *WimaController::altitude() {
QmlObjectListModel *WimaController::snakeTiles()
{
static QmlObjectListModel list;
qWarning() << "using snake tile dummy";
return QmlObjectListModel();
return &list;
}
QVariantList WimaController::snakeTileCenterPoints()
......@@ -212,11 +213,10 @@ QVariantList WimaController::snakeTileCenterPoints()
return QVariantList();
}
QVector<int> WimaController::nemoProgress() {
if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
return _nemoProgress.progress();
else
return QVector<int>(_snakeTileCenterPoints.size(), 0);
QVector<int> WimaController::nemoProgress()
{
qWarning() << "using nemoProgress dummy";
return QVector<int>();
}
double WimaController::phaseDistance() const
......@@ -231,17 +231,19 @@ double WimaController::phaseDuration() const
int WimaController::nemoStatus() const
{
return _nemoHeartbeat.status();
qWarning() << "using nemoStatus dummy";
return 0;
}
QString WimaController::nemoStatusString() const
{
return _nemoStatusMap.at(_nemoHeartbeat.status());
return _nemoStatusMap.at(nemoStatus());
}
bool WimaController::snakeCalcInProgress() const
{
return _snakeCalcInProgress;
qWarning() << "using snakeCalcInProgress dummy";
return false;
}
void WimaController::setMasterController(PlanMasterController *masterC)
......@@ -762,28 +764,6 @@ void WimaController::_eventTimerHandler()
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
}
if ( _snakeTicker.ready() ) {
if ( _enableSnake.rawValue().toBool() ) {
if ( !_pRosBridge->isRunning()) {
_pRosBridge->start();
} else if ( _pRosBridge->isRunning()
&& _pRosBridge->connected()
&& !_topicServiceSetupDone) {
if ( _doTopicServiceSetup() )
_topicServiceSetupDone = true;
} else if ( _pRosBridge->isRunning()
&& !_pRosBridge->connected()
&& _topicServiceSetupDone){
_pRosBridge->reset();
_pRosBridge->start();
_topicServiceSetupDone = false;
}
} else if ( _pRosBridge->isRunning() ) {
_pRosBridge->reset();
_topicServiceSetupDone = false;
}
}
}
void WimaController::_smartRTLCleanUp(bool flying)
......@@ -965,114 +945,4 @@ void WimaController::_switchSnakeManager(QVariant variant)
}
}
bool WimaController::_doTopicServiceSetup()
{
using namespace ros_bridge::messages;
if ( _snakeTilesLocal.polygons().size() == 0)
return false;
// Publish snake origin.
_pRosBridge->advertiseTopic("/snake/origin",
geographic_msgs::geo_point::messageType().c_str());
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
_snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator());
Q_ASSERT(ret);
(void)ret;
_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
_pRosBridge->advertiseTopic("/snake/tiles",
jsk_recognition_msgs::polygon_array::messageType().c_str());
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
ret = jsk_recognition_msgs::polygon_array::toJson(
_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
(void)ret;
_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
// Subscribe nemo progress.
_pRosBridge->subscribe("/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc){
int requiredSize = this->_snakeTilesLocal.polygons().size();
auto& progress_msg = this->_nemoProgress;
if ( !nemo_msgs::progress::fromJson(*pDoc, progress_msg)
|| progress_msg.progress().size() != requiredSize ) { // Some error occured.
// Set progress to default.
progress_msg.progress().fill(0, requiredSize);
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
this->_snakeOrigin, *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->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
(void)ret;
this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
emit WimaController::nemoProgressChanged();
});
// Subscribe /nemo/heartbeat.
_pRosBridge->subscribe("/nemo/heartbeat",
/* callback */ [this](JsonDocUPtr pDoc){
auto &heartbeat_msg = this->_nemoHeartbeat;
if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) {
if ( heartbeat_msg.status() == this->_fallbackStatus )
return;
heartbeat_msg.setStatus(this->_fallbackStatus);
}
this->_nemoTimeoutTicker.reset();
this->_fallbackStatus = -1; /*Timeout*/
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
});
// Advertise /snake/get_origin.
_pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
[this](JsonDocUPtr) -> JsonDocUPtr
{
using namespace ros_bridge::messages;
JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
::GeoPoint3D origin = this->_snakeOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
bool ret = geographic_msgs::geo_point::toJson(
origin, jOrigin, pDoc->GetAllocator());
Q_ASSERT(ret);
(void)ret;
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
return pDoc;
});
// Advertise /snake/get_tiles.
_pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
[this](JsonDocUPtr) -> JsonDocUPtr
{
JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
bool ret = jsk_recognition_msgs::polygon_array::toJson(
this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
Q_ASSERT(ret);
(void)ret;
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
return pDoc;
});
return true;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment