diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp index 69c6d9bcd13c342d895909bc2ce1de353f6e9268..92941f510e086a003c7dae1a241730d69e87ee44 100644 --- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp +++ b/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); } diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h index 40f9d4861db9abcfc9043dd26f6d454edc642fea..9aedab7bee4f27784cc02bd9f5e6d7a5562aa0cd 100644 --- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h +++ b/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; }; /**