Skip to content

Q
qgroundcontrol
Commit 591ba07e authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

123

parent e089277b
Hide whitespace changes
Inline Side-by-side
Showing with 264 additions and 1556 deletions
qgroundcontrol.pro
View file @ 591ba07e
......@@ -28,11 +28,12 @@ QGCROOT = $$PWD
DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
DEFINES += ROS_BRIDGE_DEBUG
#DEFINES += ROS_BRIDGE_CLIENT_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
DEFINES += DEBUG
DEFINES += ROS_BRIDGE_DEBUG
DEFINES += NDEBUG
}
......
src/Wima/Geometry/WimaPolygonArray.h
View file @ 591ba07e
......@@ -13,6 +13,9 @@ public:
WimaPolygonArray(const WimaPolygonArray &other) :
_polygons(other._polygons), _dirty(true)
{}
~WimaPolygonArray(){
_objs.clearAndDeleteContents();
}
class QmlObjectListModel *QmlObjectListModel(){
if (_dirty)
......@@ -34,9 +37,9 @@ public:
private:
void _updateObjects(void){
_objs.clear();
_objs.clearAndDeleteContents();
for (long i=0; i<_polygons.size(); ++i){
_objs.append(&_polygons[i]);
_objs.append(new PolygonType(_polygons[i]));
}
}
......
src/Wima/WimaController.cc
View file @ 591ba07e
......@@ -89,6 +89,7 @@ WimaController::WimaController(QObject *parent)
, _batteryLevelTicker (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
, _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/)
, _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
, _topicServiceSetupDone (false)
{
// ROS Bridge.
......@@ -551,23 +552,29 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
emit snakeTileCenterPointsChanged();
if ( _enableSnake.rawValue().toBool()
&& _pRosBridge->isRunning()
&& _pRosBridge->connected() ){
if ( _snakeTilesLocal.polygons().size() > 0 ){
using namespace ros_bridge::messages;
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
Q_ASSERT(geographic_msgs::geo_point::toJson(
_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
&& _pRosBridge->isRunning()
&& _pRosBridge->connected()
&& _topicServiceSetupDone
&& _snakeTilesLocal.polygons().size() > 0
)
{
using namespace ros_bridge::messages;
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
bool ret = geographic_msgs::geo_point::toJson(
_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
Q_ASSERT(ret);
_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(
_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
Q_ASSERT(ret);
_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
_localPlanDataValid = true;
return true;
}
......@@ -758,21 +765,24 @@ void WimaController::_eventTimerHandler()
}
if ( _snakeTicker.ready() ) {
static bool setupDone = false;
if ( _enableSnake.rawValue().toBool() ) {
if ( !_pRosBridge->isRunning()) {
_pRosBridge->start();
} else if ( _pRosBridge->isRunning() && _pRosBridge->connected() && !setupDone) {
} else if ( _pRosBridge->isRunning()
&& _pRosBridge->connected()
&& !_topicServiceSetupDone) {
if ( _doTopicServiceSetup() )
_topicServiceSetupDone = true;
} else if ( _pRosBridge->isRunning()
&& !_pRosBridge->connected()
&& _topicServiceSetupDone){
_pRosBridge->reset();
_pRosBridge->start();
_setupTopicService();
setupDone = true;
} else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){
setupDone = false;
_topicServiceSetupDone = false;
}
} else if ( _pRosBridge->isRunning() ) {
_pRosBridge->reset();
setupDone = false;
_topicServiceSetupDone = false;
}
}
}
......@@ -948,21 +958,35 @@ void WimaController::_switchSnakeManager(QVariant variant)
}
}
void WimaController::_setupTopicService()
bool WimaController::_doTopicServiceSetup()
{
using namespace ros_bridge::messages;
if ( _snakeTilesLocal.polygons().size() > 0){
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
Q_ASSERT(geographic_msgs::geo_point::toJson(
_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
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){
......@@ -971,24 +995,30 @@ void WimaController::_setupTopicService()
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 origin and tiles again, if valid.
if ( this->_snakeTilesLocal.polygons().size() > 0){
// Publish snake origin.
JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
Q_ASSERT(geographic_msgs::geo_point::toJson(
this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
// Publish snake tiles.
JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
}
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;
......@@ -1004,31 +1034,38 @@ void WimaController::_setupTopicService()
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);
if ( this->_snakeTilesLocal.polygons().size() > 0){
geographic_msgs::geo_point::toJson(
this->_snakeOrigin, jOrigin, pDoc->GetAllocator());
} else {
geographic_msgs::geo_point::toJson(
::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator());
}
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);
jsk_recognition_msgs::polygon_array::toJson(
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;
}
src/Wima/WimaController.h
View file @ 591ba07e
......@@ -337,7 +337,7 @@ private slots:
void _snakeStoreWorkerResults ();
void _initStartSnakeWorker ();
void _switchSnakeManager (QVariant variant);
void _setupTopicService ();
bool _doTopicServiceSetup();
// Periodic tasks.
void _eventTimerHandler (void);
// Waypoint manager handling.
......@@ -407,6 +407,7 @@ private:
int _fallbackStatus;
ROSBridgePtr _pRosBridge;
static StatusMap _nemoStatusMap;
bool _topicServiceSetupDone;
// Periodic tasks.
QTimer _eventTimer;
......
src/Wima/WimaController_new.cc deleted 100644 → 0
View file @ e089277b
#include "WimaController.h"
#include "utilities.h"
#include "ros_bridge/include/JsonMethodes.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/CasePacker.h"
#include "Snake/QtROSJsonFactory.h"
#include "Snake/QtROSTypeFactory.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "QVector3D"
#include <QScopedPointer>
// const char* WimaController::wimaFileExtension = "wima";
const char* WimaController::areaItemsName = "AreaItems";
const char* WimaController::missionItemsName = "MissionItems";
const char* WimaController::settingsGroup = "WimaController";
const char* WimaController::enableWimaControllerName = "EnableWimaController";
const char* WimaController::overlapWaypointsName = "OverlapWaypoints";
const char* WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase";
const char* WimaController::startWaypointIndexName = "StartWaypointIndex";
const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems";
const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems";
const char* WimaController::flightSpeedName = "FlightSpeed";
const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char* WimaController::altitudeName = "Altitude";
const char* WimaController::snakeTileWidthName = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
const char* WimaController::snakeLineDistanceName = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength";
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")};
using namespace snake;
using namespace snake_geometry;
WimaController::WimaController(QObject *parent)
: QObject (parent)
, _joinedArea ()
, _measurementArea ()
, _serviceArea ()
, _corridor ()
, _localPlanDataValid (false)
, _areaInterface (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
, _managerSettings ()
, _defaultManager (_managerSettings, _areaInterface)
, _snakeManager (_managerSettings, _areaInterface)
, _rtlManager (_managerSettings, _areaInterface)
, _currentManager (&_defaultManager)
, _managerList {&_defaultManager, &_snakeManager, &_rtlManager}
, _metaDataMap (FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
, _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName])
, _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName])
, _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
, _nextPhaseStartWaypointIndex (settingsGroup, _metaDataMap[startWaypointIndexName])
, _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName])
, _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName])
, _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName])
, _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
, _altitude (settingsGroup, _metaDataMap[altitudeName])
, _measurementPathLength (-1)
, _lowBatteryHandlingTriggered (false)
, _snakeCalcInProgress (false)
, _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName])
, _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName])
, _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName])
, _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName])
, _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
, _nemoHeartbeat (0 /*status: not connected*/)
, _fallbackStatus (0 /*status: not connected*/)
, _bridgeSetupDone (false)
, _pRosBridge (new ROSBridge::ROSBridge())
{
// Set up facts.
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_setStartIndex);
connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
// Init waypoint managers.
bool value;
size_t overlap = _overlapWaypoints.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value);
Q_ASSERT(value);
size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
Q_ASSERT(value);
(void)value;
for (auto manager : _managerList){
manager->setOverlap(overlap);
manager->setN(N);
manager->setStartIndex(startIdx);
}
// Periodic.
connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
//_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
_eventTimer.start(EVENT_TIMER_INTERVAL);
// Snake Worker Thread.
connect(&_snakeWorker, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData);
connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit);
// Snake.
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
_switchSnakeManager(_enableSnake.rawValue());
}
PlanMasterController *WimaController::masterController() {
return _masterController;
}
MissionController *WimaController::missionController() {
return _missionController;
}
QmlObjectListModel *WimaController::visualItems() {
return &_areas;
}
QmlObjectListModel *WimaController::missionItems() {
return const_cast<QmlObjectListModel*>(&_currentManager->missionItems());
}
QmlObjectListModel *WimaController::currentMissionItems() {
return const_cast<QmlObjectListModel*>(&_currentManager->currentMissionItems());
}
QVariantList WimaController::waypointPath()
{
return const_cast<QVariantList&>(_currentManager->waypointsVariant());
}
QVariantList WimaController::currentWaypointPath()
{
return const_cast<QVariantList&>(_currentManager->currentWaypointsVariant());
}
Fact *WimaController::enableWimaController() {
return &_enableWimaController;
}
Fact *WimaController::overlapWaypoints() {
return &_overlapWaypoints;
}
Fact *WimaController::maxWaypointsPerPhase() {
return &_maxWaypointsPerPhase;
}
Fact *WimaController::startWaypointIndex() {
return &_nextPhaseStartWaypointIndex;
}
Fact *WimaController::showAllMissionItems() {
return &_showAllMissionItems;
}
Fact *WimaController::showCurrentMissionItems() {
return &_showCurrentMissionItems;
}
Fact *WimaController::flightSpeed() {
return &_flightSpeed;
}
Fact *WimaController::arrivalReturnSpeed() {
return &_arrivalReturnSpeed;
}
Fact *WimaController::altitude() {
return &_altitude;
}
QVector<int> WimaController::nemoProgress() {
if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
return _nemoProgress.progress();
else
return QVector<int>(_snakeTileCenterPoints.size(), 0);
}
double WimaController::phaseDistance() const
{
return 0.0;
}
double WimaController::phaseDuration() const
{
return 0.0;
}
int WimaController::nemoStatus() const
{
return _nemoHeartbeat.status();
}
QString WimaController::nemoStatusString() const
{
return _nemoStatusMap.at(_nemoHeartbeat.status());
}
bool WimaController::snakeCalcInProgress() const
{
return _snakeCalcInProgress;
}
void WimaController::setMasterController(PlanMasterController *masterC)
{
_masterController = masterC;
_managerSettings.setMasterController(masterC);
emit masterControllerChanged();
}
void WimaController::setMissionController(MissionController *missionC)
{
_missionController = missionC;
_managerSettings.setMissionController(missionC);
emit missionControllerChanged();
}
void WimaController::nextPhase()
{
_calcNextPhase();
}
void WimaController::previousPhase()
{
if ( !_currentManager->previous() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::resetPhase()
{
if ( !_currentManager->reset() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::requestSmartRTL()
{
QString errorString("Smart RTL requested. ");
if ( !_checkSmartRTLPreCondition(errorString) ){
qgcApp()->showMessage(errorString);
return;
}
emit smartRTLRequestConfirm();
}
bool WimaController::upload()
{
auto &currentMissionItems = _defaultManager.currentMissionItems();
if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
&& currentMissionItems.count() > 0) {
emit forceUploadConfirm();
return false;
}
return forceUpload();
}
bool WimaController::forceUpload()
{
auto &currentMissionItems = _defaultManager.currentMissionItems();
if (currentMissionItems.count() < 1)
return false;
_missionController->removeAll();
// Set homeposition of settingsItem.
QmlObjectListModel* visuals = _missionController->visualItems();
MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateCurrentMissionItems(): nullptr");
return false;
}
settingsItem->setCoordinate(_managerSettings.homePosition());
// Copy mission items to _missionController.
for (int i = 1; i < currentMissionItems.count(); i++){
auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count());
}
_masterController->sendToVehicle();
return true;
}
void WimaController::removeFromVehicle()
{
_masterController->removeAllFromVehicle();
_missionController->removeAll();
}
void WimaController::executeSmartRTL()
{
forceUpload();
masterController()->managerVehicle()->startMission();
}
void WimaController::initSmartRTL()
{
_initSmartRTL();
}
void WimaController::removeVehicleTrajectoryHistory()
{
Vehicle *managerVehicle = masterController()->managerVehicle();
managerVehicle->trajectoryPoints()->clear();
}
bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF polygon2D;
toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2D;
bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
bool WimaController::setWimaPlanData(const WimaPlanData &planData)
{
// reset visual items
_areas.clear();
_defaultManager.clear();
_snakeTiles.polygons().clear();
_snakeTilesLocal.polygons().clear();
_snakeTileCenterPoints.clear();
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
_localPlanDataValid = false;
// extract list with WimaAreas
QList<const WimaAreaData*> areaList = planData.areaList();
int areaCounter = 0;
const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
for (int i = 0; i < areaList.size(); i++) {
const WimaAreaData *areaData = areaList[i];
if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
_serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
areaCounter++;
_areas.append(&_serviceArea);
continue;
}
if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
_measurementArea = *qobject_cast<const WimaMeasurementAreaData*>(areaData);
areaCounter++;
_areas.append(&_measurementArea);
continue;
}
if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
_corridor = *qobject_cast<const WimaCorridorData*>(areaData);
areaCounter++;
//_visualItems.append(&_corridor); // not needed
continue;
}
if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
_joinedArea = *qobject_cast<const WimaJoinedAreaData*>(areaData);
areaCounter++;
_areas.append(&_joinedArea);
continue;
}
if (areaCounter >= numAreas)
break;
}
if (areaCounter != numAreas) {
Q_ASSERT(false);
return false;
}
emit visualItemsChanged();
// extract mission items
QList<MissionItem> tempMissionItems = planData.missionItems();
if (tempMissionItems.size() < 1) {
qWarning("WimaController: Mission items from WimaPlaner empty!");
return false;
}
for (auto item : tempMissionItems) {
_defaultManager.push_back(item.coordinate());
}
_managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
_serviceArea.center().longitude(),
0) );
if( !_defaultManager.reset() ){
Q_ASSERT(false);
return false;
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
_localPlanDataValid = true;
_initStartSnakeWorker();
return true;
}
WimaController *WimaController::thisPointer()
{
return this;
}
bool WimaController::_calcNextPhase()
{
if ( !_currentManager->next() ) {
Q_ASSERT(false);
return false;
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
bool WimaController::_setStartIndex()
{
bool value;
_currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
return false;
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
return true;
}
void WimaController::_recalcCurrentPhase()
{
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateOverlap()
{
bool value;
_currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
assert(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateMaxWaypoints()
{
bool value;
_currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateflightSpeed()
{
bool value;
_managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateArrivalReturnSpeed()
{
bool value;
_managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_updateAltitude()
{
bool value;
_managerSettings.setAltitude(_altitude.rawValue().toDouble(&value));
Q_ASSERT(value);
(void)value;
if ( !_currentManager->update() ) {
Q_ASSERT(false);
}
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
void WimaController::_checkBatteryLevel()
{
Vehicle *managerVehicle = masterController()->managerVehicle();
WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
bool enabled = _enableWimaController.rawValue().toBool();
unsigned int minTime = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
if (managerVehicle != nullptr && enabled == true) {
Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
&& battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
if (!_lowBatteryHandlingTriggered) {
_lowBatteryHandlingTriggered = true;
if ( !(_missionController->remainingTime() <= minTime) ) {
requestSmartRTL();
}
}
}
else {
_lowBatteryHandlingTriggered = false;
}
}
}
void WimaController::_eventTimerHandler()
{
static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
// Battery level check necessary?
Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() )
_checkBatteryLevel();
// Snake flight plan update necessary?
// if ( snakeEventLoopTicker.ready() ) {
// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
// }
// }
if ( nemoStatusTicker.ready() ) {
this->_nemoHeartbeat.setStatus(_fallbackStatus);
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
}
if ( snakeTicker.ready() ) {
if ( _enableSnake.rawValue().toBool()
&& _pRosBridge->connected() ) {
if ( !_bridgeSetupDone ) {
qWarning() << "ROS Bridge setup. ";
auto start = std::chrono::high_resolution_clock::now();
_pRosBridge->start();
auto end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->start() time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_pRosBridge->publish(_snakeOrigin, "/snake/origin");
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_pRosBridge->subscribe("/nemo/progress",
/* callback */ [this](JsonDocUPtr pDoc){
int requiredSize = this->_snakeTilesLocal.polygons().size();
auto& progress = this->_nemoProgress;
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
|| progress.progress().size() != requiredSize ) {
progress.progress().fill(0, requiredSize);
}
emit WimaController::nemoProgressChanged();
});
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
start = std::chrono::high_resolution_clock::now();
_pRosBridge->subscribe("/nemo/heartbeat",
/* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){
if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
return;
this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
}
nemoStatusTicker.reset();
this->_fallbackStatus = -1; /*Timeout*/
emit WimaController::nemoStatusChanged();
emit WimaController::nemoStatusStringChanged();
});
end = std::chrono::high_resolution_clock::now();
qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
<< " ms";
_bridgeSetupDone = true;
}
} else if ( _bridgeSetupDone) {
_pRosBridge->reset();
_bridgeSetupDone = false;
}
}
}
void WimaController::_smartRTLCleanUp(bool flying)
{
if ( !flying) { // vehicle has landed
_switchWaypointManager(_defaultManager);
_missionController->removeAllFromVehicle();
_missionController->removeAll();
disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
}
}
void WimaController::_setPhaseDistance(double distance)
{
(void)distance;
// if (!qFuzzyCompare(distance, _phaseDistance)) {
// _phaseDistance = distance;
// emit phaseDistanceChanged();
// }
}
void WimaController::_setPhaseDuration(double duration)
{
(void)duration;
// if (!qFuzzyCompare(duration, _phaseDuration)) {
// _phaseDuration = duration;
// emit phaseDurationChanged();
// }
}
bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
{
if (!_localPlanDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
return false;
}
return _rtlManager.checkPrecondition(errorString);
}
void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager)
{
if (_currentManager != &manager) {
_currentManager = &manager;
disconnect(&_overlapWaypoints, &Fact::rawValueChanged,
this, &WimaController::_updateOverlap);
disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged,
this, &WimaController::_updateMaxWaypoints);
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged,
this, &WimaController::_setStartIndex);
_maxWaypointsPerPhase.setRawValue(_currentManager->N());
_overlapWaypoints.setRawValue(_currentManager->overlap());
_nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());
connect(&_overlapWaypoints, &Fact::rawValueChanged,
this, &WimaController::_updateOverlap);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged,
this, &WimaController::_updateMaxWaypoints);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged,
this, &WimaController::_setStartIndex);
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
qWarning() << "WimaController::_switchWaypointManager: statistics update missing.";
}
}
void WimaController::_initSmartRTL()
{
QString errorString;
static int attemptCounter = 0;
attemptCounter++;
if ( _checkSmartRTLPreCondition(errorString) ) {
_masterController->managerVehicle()->pauseVehicle();
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
if ( _rtlManager.update() ) { // Calculate return path.
_switchWaypointManager(_rtlManager);
attemptCounter = 0;
emit smartRTLPathConfirm();
return;
}
} else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
}
}
void WimaController::_setSnakeCalcInProgress(bool inProgress)
{
if (_snakeCalcInProgress != inProgress){
_snakeCalcInProgress = inProgress;
emit snakeCalcInProgressChanged();
}
}
void WimaController::_updateSnakeData()
{
_setSnakeCalcInProgress(false);
auto start = std::chrono::high_resolution_clock::now();
_snakeManager.clear();
const auto& r = _snakeWorker.result();
if (!r.success) {
//qgcApp()->showMessage(r.errorMessage);
return;
}
// create Mission items from r.waypoints
long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
if (n < 1) {
return;
}
// Create QVector<QGeoCoordinate> containing all waypoints;
unsigned long startIdx = r.arrivalPathIdx.last();
unsigned long endIdx = r.returnPathIdx.first();
for (unsigned long i = startIdx; i <= endIdx; ++i) {
_snakeManager.push_back(r.waypoints[int(i)]);
}
auto end = std::chrono::high_resolution_clock::now();
double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms.";
start = std::chrono::high_resolution_clock::now();
_snakeManager.update();
end = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms.";
start = std::chrono::high_resolution_clock::now();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
end = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
qWarning() << "WimaController: gui update execution time: " << duration << " ms.";
start = std::chrono::high_resolution_clock::now();
_snakeOrigin = r.origin;
_snakeTileCenterPoints = r.tileCenterPoints;
_snakeTiles = r.tiles;
_snakeTilesLocal = r.tilesLocal;
end = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
qWarning() << "WimaController: tiles copy execution time: " << duration << " ms.";
start = std::chrono::high_resolution_clock::now();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
end = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms.";
}
void WimaController::_initStartSnakeWorker()
{
if ( !_enableSnake.rawValue().toBool() )
return;
// Stop worker thread if running.
if ( _snakeWorker.isRunning() ) {
_snakeWorker.quit();
}
// Initialize _snakeWorker.
_snakeWorker.setMeasurementArea(
_measurementArea.coordinateList());
_snakeWorker.setServiceArea(
_serviceArea.coordinateList());
_snakeWorker.setCorridor(
_corridor.coordinateList());
_snakeWorker.setProgress(
_nemoProgress.progress());
_snakeWorker.setLineDistance(
_snakeLineDistance.rawValue().toDouble());
_snakeWorker.setMinTransectLength(
_snakeMinTransectLength.rawValue().toDouble());
_snakeWorker.setTileHeight(
_snakeTileHeight.rawValue().toDouble());
_snakeWorker.setTileWidth(
_snakeTileWidth.rawValue().toDouble());
_snakeWorker.setMinTileArea(
_snakeMinTileArea.rawValue().toDouble());
_setSnakeCalcInProgress(true);
// Start worker thread.
_snakeWorker.start();
}
void WimaController::_switchSnakeManager(QVariant variant)
{
if (variant.value<bool>()){
_switchWaypointManager(_snakeManager);
} else {
_switchWaypointManager(_defaultManager);
}
}
src/Wima/WimaController_new.h deleted 100644 → 0
View file @ e089277b
#pragma once
#include <QObject>
#include <QScopedPointer>
#include "QGCMapPolygon.h"
#include "QmlObjectListModel.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
#include "PlanMasterController.h"
#include "MissionController.h"
#include "SurveyComplexItem.h"
#include "SimpleMissionItem.h"
#include "MissionSettingsItem.h"
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "SettingsFact.h"
#include "WimaSettings.h"
#include "SettingsManager.h"
#include "snake.h"
#include "Snake/SnakeWorker.h"
#include "Snake/GeoPolygonArray.h"
#include "Snake/PolygonArray.h"
#include "Geometry/GeoPoint3D.h"
#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"
#include "ros_bridge/include/ROSBridge.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
#include <map>
#define CHECK_BATTERY_INTERVAL 1000 // ms
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
using namespace snake;
typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
class WimaController : public QObject
{
Q_OBJECT
enum FileType {WimaFile, PlanFile};
typedef QScopedPointer<ROSBridge::ROSBridge> ROSBridgePtr;
public:
WimaController(QObject *parent = nullptr);
// Controllers.
Q_PROPERTY(PlanMasterController* masterController
READ masterController
WRITE setMasterController
NOTIFY masterControllerChanged
)
Q_PROPERTY(MissionController* missionController
READ missionController
WRITE setMissionController
NOTIFY missionControllerChanged
)
// Wima Data.
Q_PROPERTY(QmlObjectListModel* visualItems
READ visualItems
NOTIFY visualItemsChanged
)
Q_PROPERTY(QmlObjectListModel* missionItems
READ missionItems
NOTIFY missionItemsChanged
)
Q_PROPERTY(QmlObjectListModel* currentMissionItems
READ currentMissionItems
NOTIFY currentMissionItemsChanged
)
Q_PROPERTY(QVariantList waypointPath
READ waypointPath
NOTIFY waypointPathChanged
)
Q_PROPERTY(QVariantList currentWaypointPath
READ currentWaypointPath
NOTIFY currentWaypointPathChanged
)
Q_PROPERTY(Fact* enableWimaController
READ enableWimaController
CONSTANT)
// Waypoint navigaton.
Q_PROPERTY(Fact* overlapWaypoints
READ overlapWaypoints
CONSTANT
)
Q_PROPERTY(Fact* maxWaypointsPerPhase
READ maxWaypointsPerPhase
CONSTANT
)
Q_PROPERTY(Fact* startWaypointIndex
READ startWaypointIndex
CONSTANT
)
Q_PROPERTY(Fact* showAllMissionItems
READ showAllMissionItems
CONSTANT
)
Q_PROPERTY(Fact* showCurrentMissionItems
READ showCurrentMissionItems
CONSTANT
)
// Waypoint settings.
Q_PROPERTY(Fact* flightSpeed
READ flightSpeed
CONSTANT
)
Q_PROPERTY(Fact* altitude
READ altitude
CONSTANT
)
Q_PROPERTY(Fact* arrivalReturnSpeed
READ arrivalReturnSpeed
CONSTANT
)
// Waypoint statistics.
Q_PROPERTY(double phaseDistance
READ phaseDistance
NOTIFY phaseDistanceChanged
)
Q_PROPERTY(double phaseDuration
READ phaseDuration
NOTIFY phaseDurationChanged
)
// Snake
Q_PROPERTY(Fact* enableSnake
READ enableSnake
CONSTANT
)
Q_PROPERTY(int nemoStatus
READ nemoStatus
NOTIFY nemoStatusChanged
)
Q_PROPERTY(QString nemoStatusString
READ nemoStatusString
NOTIFY nemoStatusStringChanged
)
Q_PROPERTY(bool snakeCalcInProgress
READ snakeCalcInProgress
NOTIFY snakeCalcInProgressChanged
)
Q_PROPERTY(Fact* snakeTileWidth
READ snakeTileWidth
CONSTANT
)
Q_PROPERTY(Fact* snakeTileHeight
READ snakeTileHeight
CONSTANT
)
Q_PROPERTY(Fact* snakeMinTileArea
READ snakeMinTileArea
CONSTANT
)
Q_PROPERTY(Fact* snakeLineDistance
READ snakeLineDistance
CONSTANT
)
Q_PROPERTY(Fact* snakeMinTransectLength
READ snakeMinTransectLength
CONSTANT
)
Q_PROPERTY(QmlObjectListModel* snakeTiles
READ snakeTiles
NOTIFY snakeTilesChanged
)
Q_PROPERTY(QVariantList snakeTileCenterPoints
READ snakeTileCenterPoints
NOTIFY snakeTileCenterPointsChanged
)
Q_PROPERTY(QVector<int> nemoProgress
READ nemoProgress
NOTIFY nemoProgressChanged
)
// Property accessors
// Controllers.
PlanMasterController* masterController (void);
MissionController* missionController (void);
// Wima Data
QmlObjectListModel* visualItems (void);
QGCMapPolygon joinedArea (void) const;
// Waypoints.
QmlObjectListModel* missionItems (void);
QmlObjectListModel* currentMissionItems (void);
QVariantList waypointPath (void);
QVariantList currentWaypointPath (void);
// Settings facts.
Fact* enableWimaController (void);
Fact* overlapWaypoints (void);
Fact* maxWaypointsPerPhase (void);
Fact* startWaypointIndex (void);
Fact* showAllMissionItems (void);
Fact* showCurrentMissionItems(void);
Fact* flightSpeed (void);
Fact* arrivalReturnSpeed (void);
Fact* altitude (void);
// Snake settings facts.
Fact* enableSnake (void) { return &_enableSnake; }
Fact* snakeTileWidth (void) { return &_snakeTileWidth;}
Fact* snakeTileHeight (void) { return &_snakeTileHeight;}
Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;}
Fact* snakeLineDistance (void) { return &_snakeLineDistance;}
Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;}
// Snake data.
QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();}
QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;}
QVector<int> nemoProgress (void);
int nemoStatus (void) const;
QString nemoStatusString (void) const;
bool snakeCalcInProgress (void) const;
// Smart RTL.
bool uploadOverrideRequired (void) const;
bool vehicleHasLowBattery (void) const;
// Waypoint statistics.
double phaseDistance (void) const;
double phaseDuration (void) const;
// Property setters
void setMasterController (PlanMasterController* masterController);
void setMissionController (MissionController* missionController);
bool setWimaPlanData (const WimaPlanData &planData);
// Member Methodes
Q_INVOKABLE WimaController *thisPointer();
// Waypoint navigation.
Q_INVOKABLE void nextPhase();
Q_INVOKABLE void previousPhase();
Q_INVOKABLE void resetPhase();
// Smart RTL.
Q_INVOKABLE void requestSmartRTL();
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void executeSmartRTL();
// Other.
Q_INVOKABLE void removeVehicleTrajectoryHistory();
Q_INVOKABLE bool upload();
Q_INVOKABLE bool forceUpload();
Q_INVOKABLE void removeFromVehicle();
// static Members
static const char* areaItemsName;
static const char* missionItemsName;
static const char* settingsGroup;
static const char* endWaypointIndexName;
static const char* enableWimaControllerName;
static const char* overlapWaypointsName;
static const char* maxWaypointsPerPhaseName;
static const char* startWaypointIndexName;
static const char* showAllMissionItemsName;
static const char* showCurrentMissionItemsName;
static const char* flightSpeedName;
static const char* arrivalReturnSpeedName;
static const char* altitudeName;
static const char* snakeTileWidthName;
static const char* snakeTileHeightName;
static const char* snakeMinTileAreaName;
static const char* snakeLineDistanceName;
static const char* snakeMinTransectLengthName;
signals:
// Controllers.
void masterControllerChanged (void);
void missionControllerChanged (void);
// Wima data.
void visualItemsChanged (void);
// Waypoints.
void missionItemsChanged (void);
void currentMissionItemsChanged (void);
void waypointPathChanged (void);
void currentWaypointPathChanged (void);
// Smart RTL.
void smartRTLRequestConfirm (void);
void smartRTLPathConfirm (void);
// Upload.
void forceUploadConfirm (void);
// Waypoint statistics.
void phaseDistanceChanged (void);
void phaseDurationChanged (void);
// Snake.
void snakeConnectionStatusChanged (void);
void snakeCalcInProgressChanged (void);
void snakeTilesChanged (void);
void snakeTileCenterPointsChanged (void);
void nemoProgressChanged (void);
void nemoStatusChanged (void);
void nemoStatusStringChanged (void);
private slots:
// Waypoint navigation / helper.
bool _calcNextPhase (void);
void _recalcCurrentPhase (void);
bool _calcShortestPath (const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
// Slicing parameters
bool _setStartIndex (void);
void _updateOverlap (void);
void _updateMaxWaypoints (void);
// Waypoint settings.
void _updateflightSpeed (void);
void _updateArrivalReturnSpeed (void);
void _updateAltitude (void);
// Waypoint Statistics.
void _setPhaseDistance (double distance);
void _setPhaseDuration (double duration);
// SMART RTL
void _checkBatteryLevel (void);
bool _checkSmartRTLPreCondition (QString &errorString);
void _initSmartRTL ();
void _smartRTLCleanUp (bool flying);
// Snake.
void _setSnakeCalcInProgress (bool inProgress);
void _updateSnakeData ();
void _initStartSnakeWorker ();
void _switchSnakeManager (QVariant variant);
// Periodic tasks.
void _eventTimerHandler (void);
// Waypoint manager handling.
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
private:
using StatusMap = std::map<int, QString>;
// Controllers.
PlanMasterController *_masterController;
MissionController *_missionController;
// Wima Data.
QmlObjectListModel _areas; // contains all visible areas
WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor
WimaMeasurementAreaData _measurementArea; // measurement area
WimaServiceAreaData _serviceArea; // area for supplying
WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _localPlanDataValid;
// Waypoint Managers.
WaypointManager::AreaInterface _areaInterface;
WaypointManager::Settings _managerSettings;
WaypointManager::DefaultManager _defaultManager;
WaypointManager::DefaultManager _snakeManager;
WaypointManager::RTLManager _rtlManager;
WaypointManager::ManagerBase *_currentManager;
using ManagerList = QList<WaypointManager::ManagerBase*>;
ManagerList _managerList;
// Settings Facts.
QMap<QString, FactMetaData*> _metaDataMap;
SettingsFact _enableWimaController; // enables or disables the wimaControler
SettingsFact _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases
SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints per phase
SettingsFact _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
// defining the first element of the next phase
SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not.
SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not.
SettingsFact _flightSpeed; // mission flight speed
SettingsFact _arrivalReturnSpeed; // arrival and return path speed
SettingsFact _altitude; // mission altitude
SettingsFact _enableSnake; // Enable Snake (see snake.h)
SettingsFact _snakeTileWidth;
SettingsFact _snakeTileHeight;
SettingsFact _snakeMinTileArea;
SettingsFact _snakeLineDistance;
SettingsFact _snakeMinTransectLength;
// Smart RTL.
QTimer _smartRTLTimer;
bool _lowBatteryHandlingTriggered;
// Waypoint statistics.
double _measurementPathLength; // the lenght of the phase in meters
// double _phaseDistance; // the lenth in meters of the current phase
// double _phaseDuration; // the phase duration in seconds
// Snake
bool _snakeCalcInProgress;
SnakeDataManager _snakeWorker;
GeoPoint _snakeOrigin;
GeoPolygonArray _snakeTiles; // tiles
PolygonArray _snakeTilesLocal; // tiles local coordinate system
QVariantList _snakeTileCenterPoints;
QNemoProgress _nemoProgress; // measurement progress
QNemoHeartbeat _nemoHeartbeat; // measurement progress
int _fallbackStatus;
ROSBridgePtr _pRosBridge;
bool _bridgeSetupDone;
static StatusMap _nemoStatusMap;
// Periodic tasks.
QTimer _eventTimer;
};
src/comm/ros_bridge/include/RosBridgeClient.cpp
View file @ 591ba07e
......@@ -18,18 +18,18 @@ struct Task{
void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr<WsClient> client, const std::__cxx11::string &message)
{
#ifndef DEBUG
#ifndef ROS_BRIDGE_DEBUG
(void)client_name;
#endif
if (!client->on_open)
{
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -37,7 +37,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
};
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
if (!client->on_message)
{
client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
......@@ -61,21 +61,21 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
}
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::thread client_thread([client_name, client]() {
#else
std::thread client_thread([client]() {
#endif
client->start();
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Terminated" << std::endl;
#endif
client->on_open = NULL;
client->on_message = NULL;
client->on_close = NULL;
client->on_error = NULL;
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << " thread end" << std::endl;
#endif
});
......@@ -122,7 +122,7 @@ void RosbridgeWsClient::run()
// ====================================================================================
if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "Starting new poll." << std::endl;
std::cout << "connected: " << this->isConnected->load() << std::endl;
#endif
......@@ -133,14 +133,14 @@ void RosbridgeWsClient::run()
return t.name == reset_status_task_name;
});
if ( it == task_list.end() ){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "Adding status_task" << std::endl;
#endif
// Check connection status.
auto status_set = std::make_shared<std::atomic_bool>(false);
auto status_client = std::make_shared<WsClient>(this->server_port_path);
status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "status_client opened" << std::endl;
#endif
this->isConnected->store(true);
......@@ -190,16 +190,13 @@ void RosbridgeWsClient::run()
});
if ( topics_it == task_list.end() ){
// Call /rosapi/topics service.
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "Adding reset_topics_task" << std::endl;
#endif
auto topics_set = std::make_shared<std::atomic_bool>(false);
this->callService("/rosapi/topics", [topics_set, this](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message){
#ifdef DEBUG
std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
#endif
std::unique_lock<std::mutex> lk(this->mutex);
this->available_topics = in_message->string();
lk.unlock();
......@@ -241,16 +238,13 @@ void RosbridgeWsClient::run()
});
if ( services_it == task_list.end() ){
// Call /rosapi/services service.
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "Adding reset_services_task" << std::endl;
#endif
auto services_set = std::make_shared<std::atomic_bool>(false);
this->callService("/rosapi/services", [this, services_set](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message){
#ifdef DEBUG
std::cout << "/rosapi/services: " << in_message->string() << std::endl;
#endif
std::unique_lock<std::mutex> lk(this->mutex);
this->available_services = in_message->string();
lk.unlock();
......@@ -295,26 +289,14 @@ void RosbridgeWsClient::run()
// Process tasks.
// ====================================================================================
for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
#ifdef DEBUG
std::cout << "processing task: " << task_it->name << std::endl;
#endif
if ( !task_it->expired() ){
if ( task_it->ready() ){
#ifdef DEBUG
std::cout << "executing task: " << task_it->name << std::endl;
#endif
task_it->execute();
task_it = task_list.erase(task_it);
} else {
#ifdef DEBUG
std::cout << "noting to do for task: " << task_it->name << std::endl;
#endif
++task_it;
}
} else {
#ifdef DEBUG
std::cout << "task expired: " << task_it->name << std::endl;
#endif
task_it->clear_up();
task_it = task_list.erase(task_it);
}
......@@ -328,7 +310,7 @@ void RosbridgeWsClient::run()
task_it->clear_up();
}
task_list.clear();
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "periodic thread end" << std::endl;
#endif
});
......@@ -349,9 +331,14 @@ void RosbridgeWsClient::reset()
unsubscribeAll();
unadvertiseAll();
unadvertiseAllServices();
for (auto& client : client_map)
{
removeClient(client.first);
std::unique_lock<std::mutex> lk(mutex);
for (auto it = client_map.begin(); it != client_map.end(); ) {
std::string client_name = it->first;
lk.unlock();
removeClient(client_name);
lk.lock();
it = client_map.begin();
}
}
......@@ -363,7 +350,7 @@ void RosbridgeWsClient::addClient(const std::string &client_name)
{
client_map[client_name] = std::make_shared<WsClient>(server_port_path);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << " has already been created" << std::endl;
......@@ -391,7 +378,7 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been launched.
std::shared_ptr<WsClient> client = it->second;
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::thread t([client, client_name](){
#else
std::thread t([client](){
......@@ -403,14 +390,13 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
// client->on_message = NULL;
// client->on_close = NULL;
// client->on_error = NULL;
#ifdef DEBUG
std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl;
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << " has been removed" << std::endl;
#endif
});
t.detach();
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << " has not been created" << std::endl;
......@@ -428,7 +414,7 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
{
client_map.erase(it);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << " has not been created" << std::endl;
......@@ -448,15 +434,24 @@ std::string RosbridgeWsClient::getAdvertisedServices(){
}
bool RosbridgeWsClient::topicAvailable(const std::string &topic){
#ifdef DEBUG
std::lock_guard<std::mutex> lk(mutex);
#ifdef ROS_BRIDGE_DEBUG
std::cout << "checking if topic " << topic << " is available" << std::endl;
std::cout << "available topics: " << available_topics << std::endl;
#endif
size_t pos;
{
std::lock_guard<std::mutex> lk(mutex);
pos = available_topics.find(topic);
}
return pos != std::string::npos ? true : false;
bool ret = pos != std::string::npos ? true : false;
#ifdef ROS_BRIDGE_DEBUG
if ( ret ){
std::cout << "topic " << topic << " is available" << std::endl;
} else {
std::cout << "topic " << topic << " is not available" << std::endl;
}
#endif
return ret;
}
void RosbridgeWsClient::advertise(const std::string &client_name, const std::string &topic, const std::string &type, const std::string &id)
......@@ -471,7 +466,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top != service_topic_list.end()){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
return;
......@@ -487,13 +482,13 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
}
message = "{" + message + "}";
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [this, topic, message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [this, topic, message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -502,7 +497,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
start(client_name, client, message);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
......@@ -518,7 +513,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end()){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
return;
......@@ -534,13 +529,13 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
std::string client_name = "topic_unadvertiser" + topic;
auto client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -569,7 +564,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end() ){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
#endif
return;
......@@ -588,12 +583,12 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
message = "{" + message + "}";
std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message." << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
......@@ -619,7 +614,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top != service_topic_list.end()){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
return;
......@@ -659,7 +654,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
client->on_message = callback;
this->start(client_name, client, message); // subscribe to topic
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
......@@ -675,7 +670,7 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end()){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
return;
......@@ -691,13 +686,13 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
std::string client_name = "topic_unsubscriber" + topic;
auto client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -729,7 +724,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top != service_topic_list.end()){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "service: " << service << " already advertised" << std::endl;
#endif
return;
......@@ -743,7 +738,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
it_client->second->on_message = callback;
start(client_name, it_client->second, message);
}
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
......@@ -759,7 +754,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end()){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "service: " << service << " not advertised" << std::endl;
#endif
return;
......@@ -771,13 +766,13 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
std::string client_name = "service_unadvertiser" + service;
auto client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -816,12 +811,12 @@ void RosbridgeWsClient::serviceResponse(const std::string &service, const std::s
std::string client_name = "service_response_client" + service;
std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
service_response_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
......@@ -869,7 +864,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
else
{
call_service_client->on_message = [client_name](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
std::cout << client_name << ": Sending close connection" << std::endl;
#else
......@@ -884,7 +879,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
bool RosbridgeWsClient::serviceAvailable(const std::string &service)
{
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
std::cout << "checking if service " << service << " is available" << std::endl;
#endif
size_t pos;
......@@ -892,7 +887,16 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
std::lock_guard<std::mutex> lk(mutex);
pos = available_services.find(service);
}
return pos != std::string::npos ? true : false;
bool ret = pos != std::string::npos ? true : false;
#ifdef ROS_BRIDGE_DEBUG
if ( ret ){
std::cout << "service " << service << " is available" << std::endl;
} else {
std::cout << "service " << service << " is not available" << std::endl;
}
#endif
return ret;
}
void RosbridgeWsClient::waitForService(const std::string &service)
......@@ -904,28 +908,27 @@ void RosbridgeWsClient::waitForService(const std::string &service)
void RosbridgeWsClient::waitForService(const std::string &service, const std::function<bool(void)> stop)
{
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
auto s = std::chrono::high_resolution_clock::now();
long counter = 0;
#endif
const auto poll_interval = std::chrono::milliseconds(1000);
auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
auto poll_time_point = std::chrono::high_resolution_clock::now();
while( !stop() )
{
if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
++counter;
#endif
if ( serviceAvailable(service) ){
break;
} else {
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
}
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
};
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
auto e = std::chrono::high_resolution_clock::now();
std::cout << "waitForService() " << service << " time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
......@@ -944,28 +947,27 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic)
void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function<bool(void)> stop)
{
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
auto s = std::chrono::high_resolution_clock::now();
long counter = 0;
#endif
const auto poll_interval = std::chrono::milliseconds(1000);
auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
auto poll_time_point = std::chrono::high_resolution_clock::now();
while( !stop() )
{
if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
++counter;
#endif
if ( topicAvailable(topic) ){
break;
} else {
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
}
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
};
#ifdef DEBUG
#ifdef ROS_BRIDGE_DEBUG
auto e = std::chrono::high_resolution_clock::now();
std::cout << "waitForTopic() " << topic << " time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
......
src/comm/ros_bridge/include/com_private.cpp
View file @ 591ba07e
......@@ -20,18 +20,10 @@ bool ros_bridge::com_private::getTopic(const ros_bridge::com_private::JsonDoc &d
{
if ( doc.HasMember("topic") && doc["topic"].IsString() ){
rapidjson::StringBuffer sb1;
rapidjson::Writer<rapidjson::StringBuffer> writer1(sb1);
doc.Accept(writer1);
std::cout << "getTopic doc: " << sb1.GetString() << std::endl;
rapidjson::StringBuffer sb;
rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
doc["topic"].Accept(writer);
topic = sb.GetString();
std::cout << "getTopic topic: " << sb.GetString() << std::endl;
return true;
} else
return false;
......
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
View file @ 591ba07e
#include "geopoint.h"
std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
return "geometry_msgs/GeoPoint";
return "geographic_msgs/GeoPoint";
}
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
View file @ 591ba07e
......@@ -109,9 +109,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
View file @ 591ba07e
......@@ -103,9 +103,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
View file @ 591ba07e
......@@ -57,9 +57,6 @@ bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Documen
}
value.AddMember("points", points, allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
View file @ 591ba07e
......@@ -95,9 +95,6 @@ bool _toJson(const PolygonType &poly,
}
value.AddMember("header", header, allocator);
value.AddMember("polygon", polygon, allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
View file @ 591ba07e
......@@ -183,10 +183,6 @@ namespace detail {
detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
value.AddMember("likelihood", jLikelihood, allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
View file @ 591ba07e
......@@ -32,9 +32,6 @@ template <class HeartbeatType>
bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
{
value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
View file @ 591ba07e
......@@ -41,9 +41,6 @@ bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document:
jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
}
value.AddMember("progress", jProgress, allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/std_msgs/header.h
View file @ 591ba07e
......@@ -62,9 +62,6 @@ bool toJson(const HeaderType &header,
header.frameId().length(),
allocator),
allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/messages/std_msgs/time.h
View file @ 591ba07e
......@@ -37,9 +37,6 @@ bool toJson(const TimeType &time,
{
value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
rapidjson::Value jType;
jType.SetString(messageType().c_str(), allocator);
value.AddMember("type", jType, allocator);
return true;
}
......
src/comm/ros_bridge/include/ros_bridge.h
View file @ 591ba07e
......@@ -23,13 +23,16 @@ public:
void advertiseService(const char* service,
const char* type,
const std::function<JsonDocUPtr(JsonDocUPtr)> &callback);
void advertiseTopic(const char* topic,
const char* type);
//! @brief Start ROS bridge.
void start();
//! @brief Stops ROS bridge.
void reset();
//! @return Returns true if connected to the rosbridge_server, false either.
//! @note This function can block up to 100ms. However normal execution time is smaller.
//! @note \fn calls start().
bool connected();
bool isRunning();
......
src/comm/ros_bridge/include/topic_publisher.cpp
View file @ 591ba07e
......@@ -20,8 +20,6 @@ void ros_bridge::com_private::TopicPublisher::start()
return;
_stopped->store(false);
_pThread = std::make_unique<std::thread>([this]{
// Init.
std::unordered_map<std::string, std::string> topicMap;
// Main Loop.
while( !this->_stopped->load() ){
std::unique_lock<std::mutex> lk(this->_mutex);
......@@ -39,38 +37,25 @@ void ros_bridge::com_private::TopicPublisher::start()
// Get topic and type from Json message and remove it.
std::string topic;
assert(com_private::getTopic(*pJsonDoc, topic));
assert(com_private::removeTopic(*pJsonDoc));
std::string type;
assert(com_private::getType(*pJsonDoc, type));
assert(com_private::removeType(*pJsonDoc));
// Check if topic must be advertised.
std::string clientName =
ros_bridge::com_private::_topicAdvertiserKey
+ topic;
auto it = topicMap.find(clientName);
if ( it == topicMap.end()) { // Need to advertise topic?
topicMap.insert(std::make_pair(clientName, topic));
this->_rbc.addClient(clientName);
this->_rbc.advertise(clientName, topic, type);
this->_rbc.waitForTopic(topic, [this]{
return this->_stopped->load();
}); // Wait until topic is advertised.
}
bool ret = com_private::getTopic(*pJsonDoc, topic);
assert(ret);
(void)ret;
ret = com_private::removeTopic(*pJsonDoc);
assert(ret);
(void)ret;
// Wait for topic (Rosbridge needs some time to process a advertise() request).
this->_rbc.waitForTopic(topic, [this]{
return this->_stopped->load();
});
// Publish Json message.
if ( !this->_stopped->load() )
this->_rbc.publish(topic, *pJsonDoc);
} // while loop
// Tidy up.
for (auto& it : topicMap){
this->_rbc.unadvertise(it.second);
this->_rbc.removeClient(it.first);
}
#ifdef ROS_BRIDGE_DEBUG
std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
#endif
});
}
......@@ -87,7 +72,13 @@ void ros_bridge::com_private::TopicPublisher::reset()
return;
_pThread->join();
lk.lock();
lk.lock();
// Tidy up.
for (auto& it : this->_topicMap){
this->_rbc.unadvertise(it.first);
this->_rbc.removeClient(it.second);
}
this->_topicMap.clear();
_queue.clear();
}
......@@ -95,14 +86,52 @@ void ros_bridge::com_private::TopicPublisher::publish(
ros_bridge::com_private::JsonDocUPtr docPtr,
const char *topic)
{
std::cout << "TopicPublisher \"topic\": " << topic << std::endl;
// Check if topic was advertised.
std::string t(topic);
std::unique_lock<std::mutex> lk(this->_mutex);
auto it = this->_topicMap.find(t);
if ( it == this->_topicMap.end()) { // Not yet advertised?
lk.unlock();
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "TopicPublisher: topic "
<< t << " not yet advertised" << std::endl;
#endif
return;
}
lk.unlock();
// Add topic information to json doc.
auto &allocator = docPtr->GetAllocator();
rapidjson::Value key("topic", allocator);
rapidjson::Value value(topic, allocator);
docPtr->AddMember(key, value, allocator);
std::unique_lock<std::mutex> lock(_mutex);
lk.lock();
_queue.push_back(std::move(docPtr));
lock.unlock();
lk.unlock();
_cv.notify_one(); // Wake publisher thread.
}
bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type)
{
std::unique_lock<std::mutex> lk(this->_mutex);
std::string t(topic);
auto it = this->_topicMap.find(t);
if ( it == this->_topicMap.end()) { // Need to advertise topic?
std::string clientName =
std::string(ros_bridge::com_private::_topicAdvertiserKey)
+ t;
this->_topicMap.insert(std::make_pair(t, clientName));
lk.unlock();
this->_rbc.addClient(clientName);
this->_rbc.advertise(clientName, t, type);
return true;
} else {
lk.unlock();
#if ROS_BRIDGE_DEBUG
std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl;
#endif
return false;
}
}
src/comm/ros_bridge/include/topic_publisher.h
View file @ 591ba07e
......@@ -31,9 +31,12 @@ public:
void reset();
void publish(JsonDocUPtr docPtr, const char *topic);
bool advertise(const char *topic, const char *type);
private:
using TopicMap = std::unordered_map<std::string, std::string>;
JsonQueue _queue;
TopicMap _topicMap;
std::mutex _mutex;
std::shared_ptr<std::atomic_bool> _stopped;
RosbridgeWsClient &_rbc;
......
src/comm/ros_bridge/src/ros_bridge.cpp
View file @ 591ba07e
......@@ -32,8 +32,15 @@ void ros_bridge::ROSBridge::advertiseService(const char *service,
_server.advertiseService(service, type, callback);
}
void ros_bridge::ROSBridge::advertiseTopic(const char *topic, const char *type)
{
_topicPublisher.advertise(topic, type);
}
void ros_bridge::ROSBridge::start()
{
if ( !_stopped->load() )
return;
_stopped->store(false);
_rbc.run();
_topicPublisher.start();
......@@ -44,6 +51,8 @@ void ros_bridge::ROSBridge::start()
void ros_bridge::ROSBridge::reset()
{
auto start = std::chrono::high_resolution_clock::now();
if ( _stopped->load() )
return;
_stopped->store(true);
_topicPublisher.reset();
_topicSubscriber.reset();
......@@ -56,6 +65,7 @@ void ros_bridge::ROSBridge::reset()
bool ros_bridge::ROSBridge::connected()
{
start();
return _rbc.connected();
}
......
Markdown is supported
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