Heartbeat handling added to WimaController

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 \

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

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
                         }

src/Wima/Snake/QNemoHeartbeat.h

+#pragma once
+
+#include "ros_bridge/include/GenericMessages.h"
+
+using QNemoHeartbeat = ROSBridge::GenericMessages::NemoMsgs::Heartbeat;

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,

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,

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 <QScopedPointer>
 
 
+
 // 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<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;
 
@@ -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<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;
@@ -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;
     }
 

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 <map>
+
 #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<int>                nemoProgress           (void)           { return _nemoProgress.progress();}    
-    long                        snakeConnectionStatus  (void) const;
+    QVector<int>                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<int, QString>;
     // 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;

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

src/comm/ros_bridge/include/JsonFactory.h

@@ -186,6 +186,20 @@ private:
 
         return doc;
     }
+
+    // ===========================================================================
+    // HeartbeatGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::HeartbeatGroup>){
+        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

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 <class HeartbeatType>
+    bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+    {
+        value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
+        return true;
+    }
+
+    template <class HeartbeatType>
+    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

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<MessageGroups::NemoMsgs,
                                     MessageGroups::NemoMsgs::ProgressGroup>
         ProgressGroup;
 
+typedef MessageGroups::MessageGroup<MessageGroups::NemoMsgs,
+                                    MessageGroups::NemoMsgs::HeartbeatGroup>
+        HeartbeatGroup;
+
 } // end namespace MessageGroups
 
 } // end namespace ros_bridge

src/comm/ros_bridge/include/TypeFactory.h

@@ -120,6 +120,17 @@ private:
         return ret;
     }
 
+    // ===========================================================================
+    // HeartbeatGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::HeartbeatGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::NemoMsgs::Heartbeat::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
 
 };
 

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;
 };