added reconnect for HokuyoLaserUnit

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp

@@ -168,6 +168,27 @@ void HokuyoLaserUnit::updateScanData()
     TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
     for (HokuyoLaserScanDevice& device : devices)
     {
+        if (device.errorCounter > 10)
+        {
+            ARMARX_ERROR << "Device " << device.ip  << " has too many consecutive errors!";
+            // assume dead
+            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);
+            if (device.connected)
+            {
+                ARMARX_IMPORTANT << "Reconnected succesffully to " << device.ip << ":" << device.port;
+            }
+            else
+            {
+                ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
+                               << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+                continue;
+            }
+
+        }
         if (device.connected)
         {
             int ret = urg_start_measurement(&device.urg, URG_DISTANCE, 1, 0);
@@ -175,6 +196,7 @@ void HokuyoLaserUnit::updateScanData()
             {
                 ARMARX_WARNING << deactivateSpam(1) << "Could not start measurement for laser scanner (IP: "
                                << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+                device.errorCounter++;
                 continue;
             }
 
@@ -183,6 +205,7 @@ void HokuyoLaserUnit::updateScanData()
             {
                 ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
                                << device.ip << ", Port: " << device.port << ", Error: " << lengthDataSize << ")";
+                device.errorCounter++;
                 continue;
             }
 
@@ -201,6 +224,8 @@ void HokuyoLaserUnit::updateScanData()
                 }
             }
 
+            device.errorCounter = 0;
+
             topic->reportSensorValues(device.ip, device.frame, scan, now);
             robotHealthTopic->heartbeat(getName() + "_" + device.ip);
         }

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h

@@ -60,6 +60,7 @@ namespace armarx
         bool connected = false;
         urg_t urg;
         std::vector<long> lengthData;
+        int errorCounter = 0;
     };
 
     /**