From 591ba07efbe9d9ad3399faa101614fa91536dfc0 Mon Sep 17 00:00:00 2001
From: Valentin Platzgummer <valentin.platzgummer@nt.tuwien.ac.at>
Date: Mon, 24 Aug 2020 15:41:58 +0200
Subject: [PATCH] 123
---
qgroundcontrol.pro | 3 +-
src/Wima/Geometry/WimaPolygonArray.h | 7 +-
src/Wima/WimaController.cc | 151 +--
src/Wima/WimaController.h | 3 +-
src/Wima/WimaController_new.cc | 924 ------------------
src/Wima/WimaController_new.h | 421 --------
.../ros_bridge/include/RosBridgeClient.cpp | 162 +--
src/comm/ros_bridge/include/com_private.cpp | 8 -
.../messages/geographic_msgs/geopoint.cpp | 2 +-
.../messages/geographic_msgs/geopoint.h | 3 -
.../include/messages/geometry_msgs/point32.h | 3 -
.../include/messages/geometry_msgs/polygon.h | 3 -
.../messages/geometry_msgs/polygon_stamped.h | 3 -
.../jsk_recognition_msgs/polygon_array.h | 4 -
.../include/messages/nemo_msgs/heartbeat.h | 3 -
.../include/messages/nemo_msgs/progress.h | 3 -
.../include/messages/std_msgs/header.h | 3 -
.../include/messages/std_msgs/time.h | 3 -
src/comm/ros_bridge/include/ros_bridge.h | 5 +-
.../ros_bridge/include/topic_publisher.cpp | 93 +-
src/comm/ros_bridge/include/topic_publisher.h | 3 +
src/comm/ros_bridge/src/ros_bridge.cpp | 10 +
22 files changed, 264 insertions(+), 1556 deletions(-)
delete mode 100644 src/Wima/WimaController_new.cc
delete mode 100644 src/Wima/WimaController_new.h
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 9697d48201..f3f0ceabe8 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -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
}
diff --git a/src/Wima/Geometry/WimaPolygonArray.h b/src/Wima/Geometry/WimaPolygonArray.h
index c489ab15fa..6d662ad001 100644
--- a/src/Wima/Geometry/WimaPolygonArray.h
+++ b/src/Wima/Geometry/WimaPolygonArray.h
@@ -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]));
}
}
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index 5ddcf26697..ec0f6b0f68 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -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;
}
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 24f53a796d..936e76f4bb 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -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;
diff --git a/src/Wima/WimaController_new.cc b/src/Wima/WimaController_new.cc
deleted file mode 100644
index 24bf234c53..0000000000
--- a/src/Wima/WimaController_new.cc
+++ /dev/null
@@ -1,924 +0,0 @@
-#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 ¤tMissionItems = _defaultManager.currentMissionItems();
- if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
- && currentMissionItems.count() > 0) {
- emit forceUploadConfirm();
- return false;
- }
-
- return forceUpload();
-}
-
-bool WimaController::forceUpload()
-{
- auto ¤tMissionItems = _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);
- }
-}
diff --git a/src/Wima/WimaController_new.h b/src/Wima/WimaController_new.h
deleted file mode 100644
index c2e49dcfad..0000000000
--- a/src/Wima/WimaController_new.h
+++ /dev/null
@@ -1,421 +0,0 @@
-#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;
-
-};
-
-
diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp
index 5b79aff280..b423b1abc7 100644
--- a/src/comm/ros_bridge/include/RosBridgeClient.cpp
+++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp
@@ -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()
diff --git a/src/comm/ros_bridge/include/com_private.cpp b/src/comm/ros_bridge/include/com_private.cpp
index 4d5365beb7..2dceb9ee90 100644
--- a/src/comm/ros_bridge/include/com_private.cpp
+++ b/src/comm/ros_bridge/include/com_private.cpp
@@ -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;
diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
index c6fc8a4cb4..061bea1f08 100644
--- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
+++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
@@ -1,5 +1,5 @@
#include "geopoint.h"
std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
- return "geometry_msgs/GeoPoint";
+ return "geographic_msgs/GeoPoint";
}
diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
index a28cec323c..6a5e90e2a8 100644
--- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
+++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
index e02cf27636..c6e6865fd9 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
index 2aaf3d393e..e5d815c106 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
index 83868b00cc..49dfbff575 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
index 76693e65b2..05a5802ee9 100644
--- a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
+++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
index 45c8f5afdb..6f53c59af9 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
index fa7286c8e9..12b3bbe06d 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.h b/src/comm/ros_bridge/include/messages/std_msgs/header.h
index bd33f12009..e3a6546a02 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/header.h
+++ b/src/comm/ros_bridge/include/messages/std_msgs/header.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.h b/src/comm/ros_bridge/include/messages/std_msgs/time.h
index 3160a9743c..c3f7216141 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/time.h
+++ b/src/comm/ros_bridge/include/messages/std_msgs/time.h
@@ -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;
}
diff --git a/src/comm/ros_bridge/include/ros_bridge.h b/src/comm/ros_bridge/include/ros_bridge.h
index 46d9aa59ca..86e71f099d 100644
--- a/src/comm/ros_bridge/include/ros_bridge.h
+++ b/src/comm/ros_bridge/include/ros_bridge.h
@@ -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();
diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp
index 2a705d3429..e9e9ce2b20 100644
--- a/src/comm/ros_bridge/include/topic_publisher.cpp
+++ b/src/comm/ros_bridge/include/topic_publisher.cpp
@@ -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;
+ }
+}
+
diff --git a/src/comm/ros_bridge/include/topic_publisher.h b/src/comm/ros_bridge/include/topic_publisher.h
index 8c3a21259a..cbb84fb972 100644
--- a/src/comm/ros_bridge/include/topic_publisher.h
+++ b/src/comm/ros_bridge/include/topic_publisher.h
@@ -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;
diff --git a/src/comm/ros_bridge/src/ros_bridge.cpp b/src/comm/ros_bridge/src/ros_bridge.cpp
index cbf0bf93dc..0e98af2974 100644
--- a/src/comm/ros_bridge/src/ros_bridge.cpp
+++ b/src/comm/ros_bridge/src/ros_bridge.cpp
@@ -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();
}
--
GitLab