From f391ef7aff03df604722c19c6b5d0bb7d0d958db Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Tue, 28 Jul 2020 20:52:42 +0200 Subject: [PATCH] Heartbeat handling added to WimaController --- qgroundcontrol.pro | 1 + src/FlightDisplay/FlightDisplayViewMap.qml | 6 +- src/FlightDisplay/FlightDisplayWimaMenu.qml | 9 +- src/Wima/Snake/QNemoHeartbeat.h | 5 + src/Wima/WaypointManager/RTLManager.cpp | 22 ++-- src/Wima/WaypointManager/RTLManager.h | 2 +- src/Wima/WimaController.cc | 108 ++++++++++++------ src/Wima/WimaController.h | 30 +++-- src/comm/ros_bridge/include/GenericMessages.h | 23 ++++ src/comm/ros_bridge/include/JsonFactory.h | 14 +++ src/comm/ros_bridge/include/JsonMethodes.h | 26 ++++- src/comm/ros_bridge/include/MessageGroups.h | 12 +- src/comm/ros_bridge/include/TypeFactory.h | 11 ++ src/comm/utilities.h | 34 +++--- 14 files changed, 224 insertions(+), 79 deletions(-) create mode 100644 src/Wima/Snake/QNemoHeartbeat.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 4b396dd03..5ede3adcd 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -432,6 +432,7 @@ HEADERS += \ src/Wima/Geometry/GeoPoint3D.h \ src/Wima/Geometry/Polygon2D.h \ src/Wima/Geometry/PolygonArray.h \ + src/Wima/Snake/QNemoHeartbeat.h \ src/Wima/Snake/QNemoProgress.h \ src/Wima/Snake/QtROSJsonFactory.h \ src/Wima/Snake/QtROSTypeFactory.h \ diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index baee3f32e..e51f73b44 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -244,19 +244,15 @@ FlightMap { // Add Snake tiles center points to the map MapItemView { - property bool _lengthMatching: wimaController.snakeTileCenterPoints.count - > 0 - property bool _enable: wimaController.enableWimaController.value && wimaController.enableSnake.value - && _lengthMatching model: _enable ? wimaController.snakeTileCenterPoints : 0 delegate: MapCircle{ center: modelData border.color: "transparent" - color: getColor(wimaController.snakeProgress[index]) + color: getColor(wimaController.nemoProgress[index]) radius: 0.6 opacity: 1 z: 1 diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 3842a3914..0cd0a83d6 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -204,10 +204,9 @@ Item { // Snake connection status. QGCLabel { - property int _connectionStatus: wimaController.snakeConnectionStatus - text: _connectionStatus === 1 ? - qsTr("Status: Connected") - : qsTr("Status: Not Connected") + property int index: wimaController.nemoStatus + property string statusString: wimaController.nemoStatusString + text: "Status: " + statusString Layout.fillWidth: true } @@ -370,7 +369,7 @@ Item { QGCButton { id: buttonSmartRTL text: qsTr("Smart RTL") - onClicked: wimaController.initSmartRTL(); + onClicked: wimaController.requestSmartRTL(); Layout.columnSpan: 2 Layout.fillWidth: true } diff --git a/src/Wima/Snake/QNemoHeartbeat.h b/src/Wima/Snake/QNemoHeartbeat.h new file mode 100644 index 000000000..840ab74e3 --- /dev/null +++ b/src/Wima/Snake/QNemoHeartbeat.h @@ -0,0 +1,5 @@ +#pragma once + +#include "ros_bridge/include/GenericMessages.h" + +using QNemoHeartbeat = ROSBridge::GenericMessages::NemoMsgs::Heartbeat; diff --git a/src/Wima/WaypointManager/RTLManager.cpp b/src/Wima/WaypointManager/RTLManager.cpp index ec3ad8152..21ef5773a 100644 --- a/src/Wima/WaypointManager/RTLManager.cpp +++ b/src/Wima/WaypointManager/RTLManager.cpp @@ -81,22 +81,28 @@ bool WaypointManager::RTLManager::reset() return true; } -QString WaypointManager::RTLManager::checkPrecondition() +bool WaypointManager::RTLManager::checkPrecondition(QString &errorString) { + // Be aware of the error message order! Vehicle *managerVehicle = _settings->masterController()->managerVehicle(); - if (!managerVehicle->flying()) { - return QString("Vehicle is not flying. Smart RTL not available."); + + if ( managerVehicle->isOfflineEditingVehicle() ) { + errorString.append("No vehicle connected. Smart RTL not available."); + return false; } - if (!managerVehicle->isOfflineEditingVehicle()) { - return QString("Not connected to any vehicle. Smart RTL not available."); + if ( !managerVehicle->flying() ) { + errorString.append("Vehicle is not flying. Smart RTL not available."); + return false; } - if (!_areaInterface->joinedArea()->containsCoordinate(managerVehicle->coordinate())) { - return QString("Vehicle not inside save area. Smart RTL not available."); + if ( !_areaInterface->joinedArea()->containsCoordinate( + managerVehicle->coordinate()) ) { + errorString.append("Vehicle not inside save area. Smart RTL not available."); + return false; } - return QString(); + return true; } bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c, diff --git a/src/Wima/WaypointManager/RTLManager.h b/src/Wima/WaypointManager/RTLManager.h index 9166beaf8..bacc49068 100644 --- a/src/Wima/WaypointManager/RTLManager.h +++ b/src/Wima/WaypointManager/RTLManager.h @@ -76,7 +76,7 @@ public: //! virtual bool reset() override; - QString checkPrecondition(); + bool checkPrecondition(QString &errorString); protected: bool _insertMissionItem(const QGeoCoordinate &c, diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 536c85237..b3fb20ac5 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -9,11 +9,13 @@ #include "Snake/QtROSJsonFactory.h" #include "Snake/QtROSTypeFactory.h" #include "Snake/QNemoProgress.h" +#include "Snake/QNemoHeartbeat.h" #include "QVector3D" #include + // const char* WimaController::wimaFileExtension = "wima"; const char* WimaController::areaItemsName = "AreaItems"; const char* WimaController::missionItemsName = "MissionItems"; @@ -33,6 +35,10 @@ const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea"; const char* WimaController::snakeLineDistanceName = "SnakeLineDistance"; const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength"; +WimaController::StatusMap WimaController::_nemoStatusMap{std::make_pair(0, "No Heartbeat"), + std::make_pair(1, "Connected"), + std::make_pair(-1, "Timeout")}; + using namespace snake; using namespace snake_geometry; @@ -61,7 +67,6 @@ WimaController::WimaController(QObject *parent) , _altitude (settingsGroup, _metaDataMap[altitudeName]) , _measurementPathLength (-1) , _lowBatteryHandlingTriggered (false) - , _snakeConnectionStatus (SnakeConnectionStatus::Connected) // TODO: implement automatic connection , _snakeCalcInProgress (false) , _scenarioDefinedBool (false) , _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName]) @@ -69,6 +74,8 @@ WimaController::WimaController(QObject *parent) , _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName]) , _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName]) , _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName]) + , _nemoHeartbeat (0 /*status: not connected*/) + , _fallbackStatus (0 /*status: not connected*/) , _pRosBridge (new ROSBridge::ROSBridge()) { // Set up facts. @@ -85,16 +92,17 @@ WimaController::WimaController(QObject *parent) _defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt()); _defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt()); - // setup low battery handling + // Periodic. connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler); - _eventTimer.setInterval(EVENT_TIMER_INTERVAL); + //_eventTimer.setInterval(EVENT_TIMER_INTERVAL); + _eventTimer.start(EVENT_TIMER_INTERVAL); // Snake Worker Thread. connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit); - // Start, stop RosBridge. + // Snake. connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge); _startStopRosBridge(); connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker); @@ -169,6 +177,13 @@ Fact *WimaController::altitude() { return &_altitude; } +QVector WimaController::nemoProgress() { + if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() ) + return _nemoProgress.progress(); + else + return QVector(_snakeTileCenterPoints.size(), 0); +} + double WimaController::phaseDistance() const { return 0.0; @@ -179,9 +194,14 @@ double WimaController::phaseDuration() const return 0.0; } -long WimaController::snakeConnectionStatus() const +int WimaController::nemoStatus() const +{ + return _nemoHeartbeat.status(); +} + +QString WimaController::nemoStatusString() const { - return _snakeConnectionStatus; + return _nemoStatusMap.at(_nemoHeartbeat.status()); } bool WimaController::snakeCalcInProgress() const @@ -234,6 +254,11 @@ void WimaController::resetPhase() void WimaController::requestSmartRTL() { + QString errorString("Smart RTL requested. "); + if ( !_checkSmartRTLPreCondition(errorString) ){ + qgcApp()->showMessage(errorString); + return; + } emit smartRTLRequestConfirm(); } @@ -521,7 +546,7 @@ bool WimaController::_calcNextPhase() bool WimaController::_setStartIndex() { bool value; - _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)); + _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1); Q_ASSERT(value); (void)value; @@ -587,7 +612,7 @@ void WimaController::_updateMaxWaypoints() void WimaController::_updateflightSpeed() { bool value; - _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble()); + _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value)); Q_ASSERT(value); (void)value; @@ -604,7 +629,7 @@ void WimaController::_updateflightSpeed() void WimaController::_updateArrivalReturnSpeed() { bool value; - _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble()); + _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value)); Q_ASSERT(value); (void)value; @@ -621,7 +646,7 @@ void WimaController::_updateArrivalReturnSpeed() void WimaController::_updateAltitude() { bool value; - _managerSettings.setAltitude(_altitude.rawValue().toDouble()); + _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value)); Q_ASSERT(value); (void)value; @@ -653,7 +678,7 @@ void WimaController::_checkBatteryLevel() if (!_lowBatteryHandlingTriggered) { _lowBatteryHandlingTriggered = true; if ( !(_missionController->remainingTime() <= minTime) ) { - _initSmartRTL(); + requestSmartRTL(); } } } @@ -669,6 +694,7 @@ void WimaController::_eventTimerHandler() static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL); static EventTicker snakeEventLoopTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL); static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000); + static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000); // Battery level check necessary? Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling(); @@ -681,17 +707,35 @@ void WimaController::_eventTimerHandler() // } // } - if (rosBridgeTicker.ready()) { + if ( nemoStatusTicker.ready() ) { + this->_nemoHeartbeat.setStatus(_fallbackStatus); + emit WimaController::nemoStatusChanged(); + emit WimaController::nemoStatusStringChanged(); + } + + if (_enableSnake.rawValue().toBool() && rosBridgeTicker.ready()) { _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles"); _pRosBridge->publish(_snakeOrigin, "/snake/origin"); using namespace std::placeholders; - auto callBack = std::bind(&WimaController::_progressFromJson, - this, - _1, - std::ref(_nemoProgress)); - _pRosBridge->subscribe("/nemo/progress", callBack); + auto progressCallBack = std::bind(&WimaController::_progressFromJson, + this, + _1, + std::ref(_nemoProgress)); + _pRosBridge->subscribe("/nemo/progress", progressCallBack); + _pRosBridge->subscribe("/nemo/heartbeat", [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(); + }); } } @@ -725,14 +769,14 @@ void WimaController::_setPhaseDuration(double duration) // } } -QString WimaController::_checkSmartRTLPreCondition(void) +bool WimaController::_checkSmartRTLPreCondition(QString &errorString) { if (!_localPlanDataValid) { - return QString(tr("No WiMA data available. Please define at least a measurement and a service area.")); + errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area.")); + return false; } - auto errorString = _rtlManager.checkPrecondition(); - return errorString; + return _rtlManager.checkPrecondition(errorString); } void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager) @@ -740,6 +784,15 @@ void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manage if (_currentManager != &manager) { _currentManager = &manager; + bool value; + _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); + Q_ASSERT(value); + _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); + Q_ASSERT(value); + _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1); + Q_ASSERT(value); + (void)value; + emit missionItemsChanged(); emit currentMissionItemsChanged(); emit waypointPathChanged(); @@ -755,7 +808,7 @@ void WimaController::_initSmartRTL() static int attemptCounter = 0; attemptCounter++; - if (!_checkSmartRTLPreCondition().isEmpty()) { + if ( _checkSmartRTLPreCondition(errorString) ) { _masterController->managerVehicle()->pauseVehicle(); connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); if ( _rtlManager.update() ) { // Calculate return path. @@ -773,14 +826,6 @@ void WimaController::_initSmartRTL() } } -void WimaController::_setSnakeConnectionStatus(WimaController::SnakeConnectionStatus status) -{ - if (_snakeConnectionStatus != status) { - _snakeConnectionStatus = status; - emit snakeConnectionStatusChanged(); - } -} - void WimaController::_setSnakeCalcInProgress(bool inProgress) { if (_snakeCalcInProgress != inProgress){ @@ -823,8 +868,7 @@ void WimaController::_snakeStoreWorkerResults() // create Mission items from r.waypoints long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2; - if (n >= 1) { - Q_ASSERT(n >= 1); + if (n < 1) { return; } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index e1c49ae2d..60536124a 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -33,12 +33,15 @@ #include "Snake/SnakeTilesLocal.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 + #define CHECK_BATTERY_INTERVAL 1000 // ms #define SMART_RTL_MAX_ATTEMPTS 3 // times #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms @@ -60,8 +63,6 @@ class WimaController : public QObject public: - enum SnakeConnectionStatus {Connected = 1, NotConnected = 0}; - WimaController(QObject *parent = nullptr); @@ -149,9 +150,13 @@ public: READ enableSnake CONSTANT ) - Q_PROPERTY(long snakeConnectionStatus - READ snakeConnectionStatus - NOTIFY snakeConnectionStatusChanged + Q_PROPERTY(int nemoStatus + READ nemoStatus + NOTIFY nemoStatusChanged + ) + Q_PROPERTY(QString nemoStatusString + READ nemoStatusString + NOTIFY nemoStatusStringChanged ) Q_PROPERTY(bool snakeCalcInProgress READ snakeCalcInProgress @@ -224,8 +229,9 @@ public: // Snake data. QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();} QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;} - QVector nemoProgress (void) { return _nemoProgress.progress();} - long snakeConnectionStatus (void) const; + QVector nemoProgress (void); + int nemoStatus (void) const; + QString nemoStatusString (void) const; bool snakeCalcInProgress (void) const; // Smart RTL. @@ -303,6 +309,8 @@ signals: void snakeTilesChanged (void); void snakeTileCenterPointsChanged (void); void nemoProgressChanged (void); + void nemoStatusChanged (void); + void nemoStatusStringChanged (void); private slots: @@ -325,11 +333,10 @@ private slots: void _setPhaseDuration (double duration); // SMART RTL void _checkBatteryLevel (void); - QString _checkSmartRTLPreCondition (void); + bool _checkSmartRTLPreCondition (QString &errorString); void _initSmartRTL (); void _smartRTLCleanUp (bool flying); // Snake. - void _setSnakeConnectionStatus (SnakeConnectionStatus status); void _setSnakeCalcInProgress (bool inProgress); bool _isScenarioDefined (void); bool _isScenarioDefinedErrorMessage (void); @@ -343,6 +350,7 @@ private slots: void _switchWaypointManager(WaypointManager::ManagerBase &manager); private: + using StatusMap = std::map; // Snake. void _progressFromJson (JsonDocUPtr pDoc, QNemoProgress &progress); @@ -397,7 +405,6 @@ private: // double _phaseDuration; // the phase duration in seconds // Snake - SnakeConnectionStatus _snakeConnectionStatus; bool _snakeCalcInProgress; bool _snakeRecalcNecessary; bool _scenarioDefinedBool; @@ -408,7 +415,10 @@ private: SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system QVariantList _snakeTileCenterPoints; QNemoProgress _nemoProgress; // measurement progress + QNemoHeartbeat _nemoHeartbeat; // measurement progress + int _fallbackStatus; ROSBridgePtr _pRosBridge; + static StatusMap _nemoStatusMap; // Periodic tasks. QTimer _eventTimer; diff --git a/src/comm/ros_bridge/include/GenericMessages.h b/src/comm/ros_bridge/include/GenericMessages.h index 0b2d96d90..ae381409e 100644 --- a/src/comm/ros_bridge/include/GenericMessages.h +++ b/src/comm/ros_bridge/include/GenericMessages.h @@ -343,6 +343,29 @@ protected: typedef GenericProgress<> Progress; +// ============================================================================== +// class Heartbeat +// ============================================================================== + +//! @brief C++ representation of nemo_msgs/Heartbeat message +class Heartbeat : public MessageBaseClass{ +public: + typedef MessageGroups::HeartbeatGroup Group; + Heartbeat(){} + Heartbeat(int status) :_status(status){} + Heartbeat(const Heartbeat &heartbeat) :_status(heartbeat.status()){} + ~Heartbeat() = default; + + virtual Heartbeat *Clone() const override { return new Heartbeat(*this); } + + virtual int status (void)const {return _status;} + virtual void setStatus (int status) {_status = status;} +protected: + int _status; +}; + +typedef GenericProgress<> Progress; + } // namespace NemoMsgs } // namespace GenericMessages } // namespace ROSBridge diff --git a/src/comm/ros_bridge/include/JsonFactory.h b/src/comm/ros_bridge/include/JsonFactory.h index 2de533126..e3e1d6237 100644 --- a/src/comm/ros_bridge/include/JsonFactory.h +++ b/src/comm/ros_bridge/include/JsonFactory.h @@ -186,6 +186,20 @@ private: return doc; } + + // =========================================================================== + // HeartbeatGroup + // =========================================================================== + template + rapidjson::Document *_create(const U &msg, Type2Type){ + using namespace ROSBridge; + rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType); + bool ret = JsonMethodes::NemoMsgs::Heartbeat::toJson(msg, *doc, doc->GetAllocator()); + assert(ret); + (void)ret; + + return doc; + } }; class StdHeaderPolicy diff --git a/src/comm/ros_bridge/include/JsonMethodes.h b/src/comm/ros_bridge/include/JsonMethodes.h index 65d5e5564..9444a4c33 100644 --- a/src/comm/ros_bridge/include/JsonMethodes.h +++ b/src/comm/ros_bridge/include/JsonMethodes.h @@ -643,7 +643,7 @@ namespace Progress { { rapidjson::Value progressJson(rapidjson::kArrayType); for(unsigned long i=0; i < std::uint64_t(p.progress().size()); ++i){ - progressJson.PushBack(rapidjson::Value().SetInt(std::int8_t(p.progress()[i])), allocator); + progressJson.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator); } value.AddMember("progress", progressJson, allocator); return true; @@ -662,10 +662,32 @@ namespace Progress { p.progress().clear(); p.progress().reserve(sz); for (unsigned long i=0; i < sz; ++i) - p.progress().push_back(std::int8_t(jsonProgress[i].GetInt())); + p.progress().push_back(std::int32_t(jsonProgress[i].GetInt())); return true; } } // namespace Progress + +//! @brief Namespace containing methodes for nemo_msgs/Heartbeat generation. +namespace Heartbeat { + template + bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator) + { + value.AddMember("status", std::int32_t(heartbeat.status()), allocator); + return true; + } + + template + bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) + { + if (!value.HasMember("status") || !value["status"].IsInt()){ + assert(false); + return false; + } + + heartbeat.setStatus(value["status"].GetInt()); + return true; + } +} // namespace Heartbeat } // namespace NemoMsgs } // namespace JsonMethodes } // namespace ROSBridge diff --git a/src/comm/ros_bridge/include/MessageGroups.h b/src/comm/ros_bridge/include/MessageGroups.h index 0fdf84728..8572183e1 100644 --- a/src/comm/ros_bridge/include/MessageGroups.h +++ b/src/comm/ros_bridge/include/MessageGroups.h @@ -98,11 +98,15 @@ struct NemoMsgs { //! //! \brief The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message. - //! - //! The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message. struct ProgressGroup { static StringType label() {return "Progress";} }; + + //! + //! \brief The HeartbeatGroup struct is used the mark a class as a ROS nemo_msgs/Heartbeat message. + struct HeartbeatGroup { + static StringType label() {return "Heartbeat";} + }; }; @@ -134,6 +138,10 @@ typedef MessageGroups::MessageGroup ProgressGroup; +typedef MessageGroups::MessageGroup + HeartbeatGroup; + } // end namespace MessageGroups } // end namespace ros_bridge diff --git a/src/comm/ros_bridge/include/TypeFactory.h b/src/comm/ros_bridge/include/TypeFactory.h index ca1544039..44f473827 100644 --- a/src/comm/ros_bridge/include/TypeFactory.h +++ b/src/comm/ros_bridge/include/TypeFactory.h @@ -120,6 +120,17 @@ private: return ret; } + // =========================================================================== + // HeartbeatGroup + // =========================================================================== + template + bool _create(const rapidjson::Document &doc, U &type, Type2Type){ + using namespace ROSBridge; + bool ret = JsonMethodes::NemoMsgs::Heartbeat::fromJson(doc, type); + assert(ret); + return ret; + } + }; diff --git a/src/comm/utilities.h b/src/comm/utilities.h index 9096bde28..a3556846a 100644 --- a/src/comm/utilities.h +++ b/src/comm/utilities.h @@ -21,25 +21,25 @@ public: //! //! \brief EventTicker Constructor used for initilization. The function \fn must not be called if this constructor is used. //! \param timerInterval The timer interval of the timer provided by the user. - //! \param tickInterval The thick intervall (timerInterval <= tickInterval), see \fn ready(). + //! \param targetTime The thick intervall (timerInterval <= targetTime), see \fn ready(). //! - EventTicker(double timerInterval, double tickInterval) { + EventTicker(double timerInterval, double targetTime) { _isInitialized = false; - init(timerInterval, tickInterval); + init(timerInterval, targetTime); } //! //! \brief init Used to init the EventTicker. //! \param timerInterval The timer interval of the timer provided by the user. - //! \param tickInterval The thick intervall (timerInterval <= tickInterval), see \fn ready(). + //! \param targetTime The thick intervall (timerInterval <= targetTime), see \fn ready(). //! - void init(double timerInterval, double tickInterval){ + void init(double timerInterval, double targetTime){ assert(_isInitialized == false); assert(timerInterval > 0); - assert(tickInterval >= timerInterval); + assert(targetTime >= timerInterval); _timerInterval = timerInterval; - _tickInerval = tickInterval; - _tickCount = 0; + _targetTime = targetTime; + _ticks = 0; _isInitialized = true; } @@ -48,22 +48,28 @@ public: //! \return True if the event is ready to be executed, false either. //! //! This function must be called by a user provided timer interrupt handler. A internal counter increases with every call. - //! If _tickInerval <= _timerInterval*_tickCount the counter gets reset and the function returns true. If the condition is not + //! If _targetTime <= _timerInterval*_ticks the counter gets reset and the function returns true. If the condition is not //! met the function returns false. //! bool ready(void) { assert(_isInitialized == true); - _tickCount++; - bool ready = _tickInerval <= _timerInterval*_tickCount; + _ticks++; + bool ready = _targetTime <= _timerInterval*_ticks; if (ready) - _tickCount = 0; + _ticks = 0; return ready; } + //! + //! \brief Resets the tick count. + void reset(void) { + _ticks = 0; + } + private: double _timerInterval; - double _tickInerval; - unsigned long _tickCount; + double _targetTime; + unsigned long _ticks; bool _isInitialized; }; -- 2.22.0