diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 03b9f860ca100b7ae6a95b8a54ff39a2423f6ea0..3edfe9f89f5c8fb01a312ae2d5d303fca6c455e0 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -103,6 +103,14 @@ void HokuyoLaserUnit::onConnectComponent()
<< device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
continue;
}
+ ret = urg_start_measurement(&device.urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
+ device.connected = (ret == 0);
+ if (!device.connected)
+ {
+ ARMARX_WARNING << "Could not start measurement for laser scanner device using URG driver (IP: "
+ << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+ continue;
+ }
LaserScannerInfo info;
info.device = device.ip;
@@ -123,8 +131,7 @@ void HokuyoLaserUnit::onConnectComponent()
<< info.minAngle << ", " << info.maxAngle << ", " << info.stepSize;
}
- task = new PeriodicTask<HokuyoLaserUnit>(this, &HokuyoLaserUnit::updateScanData, updatePeriod, false, "HokuyoLaserScanUpdate");
- task->setDelayWarningTolerance(100);
+ task = new RunningTask<HokuyoLaserUnit>(this, &HokuyoLaserUnit::updateScanData, "HokuyoLaserScanUpdate");
task->start();
}
@@ -170,77 +177,99 @@ LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current& c)
void HokuyoLaserUnit::updateScanData()
{
- LaserScan scan;
- TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
- for (HokuyoLaserScanDevice& device : devices)
+ while (!task->isStopped())
{
- if (device.errorCounter > 10)
+ LaserScan scan;
+ TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
+ for (HokuyoLaserScanDevice& device : devices)
{
- ARMARX_ERROR << "Device " << device.ip << " has too many consecutive errors!";
- // assume dead
- if (device.connected)
+ IceUtil::Time time_start = TimeUtil::GetTime();
+
+ if (device.errorCounter > 10)
{
- ARMARX_INFO << "Disconnecting from laser scanner with IP " << device.ip;
- urg_close(&device.urg);
- device.connected = false;
+ ARMARX_ERROR << "Device " << device.ip << " has too many consecutive errors!";
+ // assume dead
+ 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);
+ if (!device.connected)
+ {
+ ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
+ << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+ continue;
+ }
+ ret = urg_start_measurement(&device.urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
+ device.connected = (ret == 0);
+ if (!device.connected)
+ {
+ ARMARX_WARNING << "Could not start measurement for laser scanner device using URG driver (IP: "
+ << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+ continue;
+ }
+ if (device.connected)
+ {
+ ARMARX_IMPORTANT << "Reconnected succesffully to " << device.ip << ":" << device.port;
+ }
}
- 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;
- }
+ int lengthDataSize = urg_get_distance(&device.urg, device.lengthData.data(), nullptr);
+ if (lengthDataSize < 0)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
+ << device.ip << ", Port: " << device.port << ", Error: " << lengthDataSize << ")";
+ device.errorCounter++;
+ continue;
+ }
+ IceUtil::Time time_measure = TimeUtil::GetTime();
- }
- if (device.connected)
- {
- int ret = urg_start_measurement(&device.urg, URG_DISTANCE, 1, 0);
- if (ret != 0)
- {
- ARMARX_WARNING << deactivateSpam(1) << "Could not start measurement for laser scanner (IP: "
- << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
- device.errorCounter++;
- continue;
- }
+ scan.clear();
+ scan.reserve(lengthDataSize);
+ for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
+ {
+ LaserScanStep step;
+ long distance = device.lengthData[stepIndex];
+ // TODO: Extract the min and max valid value for distance into parameters?
+ if (distance >= 21 && distance <= 30000)
+ {
+ step.angle = angleOffset + (float)urg_index2rad(&device.urg, stepIndex); // Convert steps to rad
+ step.distance = (float)distance; // Data is already in mm
+ scan.push_back(step);
+ }
+ }
- int lengthDataSize = urg_get_distance(&device.urg, device.lengthData.data(), nullptr);
- if (lengthDataSize < 0)
- {
- ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
- << device.ip << ", Port: " << device.port << ", Error: " << lengthDataSize << ")";
- device.errorCounter++;
- continue;
- }
+ IceUtil::Time time_update = TimeUtil::GetTime();
- scan.clear();
- scan.reserve(lengthDataSize);
- for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
- {
- LaserScanStep step;
- long distance = device.lengthData[stepIndex];
- // TODO: Extract the min and max valid value for distance into parameters?
- if (distance >= 21 && distance <= 30000)
+ device.errorCounter = 0;
+
+ topic->reportSensorValues(device.ip, device.frame, scan, now);
+ IceUtil::Time time_topicSensor = TimeUtil::GetTime();
+
+ RobotHealthHeartbeatArgs args;
+ args.maximumCycleTimeWarningMS = 500;
+ args.maximumCycleTimeErrorMS = 800;
+ robotHealthTopic->heartbeat(getName() + "_" + device.ip, args);
+
+ IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
+
+ IceUtil::Time duration = time_topicHeartbeat - time_start;
+ if (duration.toSecondsDouble() > 0.1)
{
- step.angle = angleOffset + (float)urg_index2rad(&device.urg, stepIndex); // Convert steps to rad
- step.distance = (float)distance; // Data is already in mm
- scan.push_back(step);
+ ARMARX_WARNING << "Laserscanner reports are slow"
+ << "Total time: " << duration.toMilliSecondsDouble() << "ms\n"
+ << "Measure: " << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
+ << "Update: " << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
+ << "TopSensor: " << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
+ << "TopHeart: " << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble() << "ms\n";
+
}
}
-
- device.errorCounter = 0;
-
- topic->reportSensorValues(device.ip, device.frame, scan, now);
- RobotHealthHeartbeatArgs args;
- args.maximumCycleTimeWarningMS = 500;
- args.maximumCycleTimeErrorMS = 800;
- robotHealthTopic->heartbeat(getName() + "_" + device.ip, args);
}
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 9aedab7bee4f27784cc02bd9f5e6d7a5562aa0cd..298bb5f66ab8c84a28991132ea97e4ff7915b7d8 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -23,7 +23,7 @@
#pragma once
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
@@ -126,7 +126,7 @@ namespace armarx
int updatePeriod = 25;
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
- PeriodicTask<HokuyoLaserUnit>::pointer_type task;
+ RunningTask<HokuyoLaserUnit>::pointer_type task;
LaserScannerInfoSeq connectedDevices;
RobotHealthInterfacePrx robotHealthTopic;
};