added haertbeat for laser scanners (uncommited from robot)

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp

@@ -23,6 +23,7 @@
 #include "HokuyoLaserUnit.h"
 
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <sys/resource.h>
 
 #include <boost/algorithm/string/split.hpp>
 #include <HokuyoLaserScannerDriver/urg_utils.h>
@@ -32,6 +33,11 @@ using namespace armarx;
 
 void HokuyoLaserUnit::onInitComponent()
 {
+    //    ARMARX_INFO << "Priority: " << getpriority(PRIO_PROCESS, LogSender::getProcessId());
+    //    //    setpriority(PRIO_PROCESS, LogSender::getProcessId(), -10);
+    //    int prio = -20;
+    //    auto ret = nice(prio);
+    //    ARMARX_INFO << "Priority after: " << getpriority(PRIO_PROCESS, LogSender::getProcessId()) << " ret: " << ret;
     offeringTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
 
     topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
@@ -172,8 +178,12 @@ void HokuyoLaserUnit::updateScanData()
         {
             ARMARX_ERROR << "Device " << device.ip  << " has too many consecutive errors!";
             // assume dead
-            device.connected = false;
-
+            if (device.connected)
+            {
+                ARMARX_INFO << "Disconnecting from laser scanner with IP " << device.ip;
+                urg_close(&device.urg);
+                device.connected = false;
+            }
             ARMARX_WARNING << "Reconnecting to " << device.ip << ":" << device.port;
             int ret = urg_open(&device.urg, URG_ETHERNET, device.ip.c_str(), device.port);
             device.connected = (ret == 0);
@@ -227,7 +237,10 @@ void HokuyoLaserUnit::updateScanData()
             device.errorCounter = 0;
 
             topic->reportSensorValues(device.ip, device.frame, scan, now);
-            robotHealthTopic->heartbeat(getName() + "_" + device.ip, RobotHealthHeartbeatArgs());
+            RobotHealthHeartbeatArgs args;
+            args.maximumCycleTimeWarningMS = 500;
+            args.maximumCycleTimeErrorMS = 800;
+            robotHealthTopic->heartbeat(getName() + "_" + device.ip, args);
         }
     }
 }