From a38cc15f856dc80b63930f2fc350faf98841178f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 28 Jul 2023 09:55:58 +0200
Subject: [PATCH] Change topic to proxy connection for Hokuyo unit
---
.../HokuyoLaserUnit/HokuyoLaserUnit.cpp | 34 +++++++++++--------
.../drivers/HokuyoLaserUnit/HokuyoLaserUnit.h | 8 ++---
2 files changed, 23 insertions(+), 19 deletions(-)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 2a1f02718..a89af6168 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -24,6 +24,7 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -45,9 +46,9 @@ HokuyoLaserUnit::onInitComponent()
offeringTopicFromProperty("RobotHealthTopicName");
offeringTopicFromProperty("DebugObserverName");
- topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
- offeringTopic(topicName);
- ARMARX_INFO << "Going to report on topic " << topicName;
+ laserScannerListenerProxyName = getProperty<std::string>("LaserScannerListenerName").getValue();
+ usingProxyFromProperty("LaserScannerListenerName");
+ ARMARX_INFO << "Going to report to listener " << laserScannerListenerProxyName;
updatePeriod = getProperty<int>("UpdatePeriod").getValue();
angleOffset = getProperty<float>("AngleOffset").getValue();
@@ -92,7 +93,7 @@ HokuyoLaserUnit::onConnectComponent()
{
ARMARX_TRACE;
- topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
+ getProxyFromProperty(listenerPrx, "LaserScannerListenerName");
debugObserver = getTopic<DebugObserverInterfacePrx>(
getProperty<std::string>("DebugObserverName").getValue());
@@ -133,7 +134,7 @@ HokuyoLaserUnit::onConnectComponent()
info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
device.lengthData.resize(lengthDataSize);
- device.scanTopic = topic;
+ device.listenerPrx = listenerPrx;
device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver;
@@ -181,7 +182,7 @@ HokuyoLaserUnit::createPropertyDefinitions()
std::string
HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
- return topicName;
+ return laserScannerListenerProxyName;
}
LaserScannerInfoSeq
@@ -230,6 +231,8 @@ HokuyoLaserScanDevice::run()
ARMARX_TRACE;
while (!task->isStopped())
{
+ ARMARX_INFO << deactivateSpam() << "Running update loop for laserscanner device " << ip;
+
IceUtil::Time time_start = TimeUtil::GetTime();
if (errorCounter > 10)
@@ -272,15 +275,16 @@ HokuyoLaserScanDevice::run()
errorCounter = 0;
- if (scanTopic)
+ if (listenerPrx)
{
- scanTopic->reportSensorValues(ip, frame, scan, now);
+
+ listenerPrx->reportSensorValues(ip, frame, scan, now);
}
else
{
- ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
+ ARMARX_WARNING << "No proxy available: IP: " << ip << ", Port: " << port;
}
- IceUtil::Time time_topicSensor = TimeUtil::GetTime();
+ IceUtil::Time time_proxyReport = TimeUtil::GetTime();
if (robotHealthPlugin != nullptr)
{
@@ -302,10 +306,10 @@ HokuyoLaserScanDevice::run()
new Variant((time_measure - time_start).toMilliSecondsDouble());
durations["update_ms"] =
new Variant((time_update - time_measure).toMilliSecondsDouble());
- durations["topic_sensor_ms"] =
- new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
+ durations["proxy_report_ms"] =
+ new Variant((time_proxyReport - time_update).toMilliSecondsDouble());
durations["topic_health_ms"] =
- new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
+ new Variant((time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble());
debugObserver->setDebugChannel(
"LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
@@ -318,9 +322,9 @@ HokuyoLaserScanDevice::run()
<< "Update: "
<< (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
<< "TopicSensor: "
- << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
+ << (time_proxyReport - time_update).toMilliSecondsDouble() << "ms\n"
<< "TopicHeart: "
- << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble()
+ << (time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble()
<< "ms\n";
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 0b8ad3985..98da02fce 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -47,7 +47,7 @@ namespace armarx
armarx::ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+ "LaserScannerListenerName", "CartographerMappingAndLocalization", "Name of the laser scan listener.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset",
-2.3561944902,
@@ -87,7 +87,7 @@ namespace armarx
LaserScan scan;
std::string componentName;
- LaserScannerUnitListenerPrx scanTopic;
+ LaserScannerUnitListenerPrx listenerPrx;
armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver;
};
@@ -150,8 +150,8 @@ namespace armarx
LaserScannerInfoSeq getConnectedDevices(const Ice::Current& c) const override;
private:
- std::string topicName;
- LaserScannerUnitListenerPrx topic;
+ std::string laserScannerListenerProxyName;
+ LaserScannerUnitListenerPrx listenerPrx;
int updatePeriod = 25;
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
--
GitLab