Skip to content
Snippets Groups Projects
Commit 2665f2e5 authored by Fabian Paus's avatar Fabian Paus
Browse files

HokuyoLaserUnit: 

+ Add timing for long calls
+ Use infinite reporting for the scanners
parent 066e6852
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+ 90
− 61
View file @ 2665f2e5
......@@ -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);
}
}
}
......
source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+ 2
− 2
View file @ 2665f2e5
......@@ -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;
};
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment