diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp index 2a1f02718bb8b1fa9465445086238a4e04ba92dd..a89af6168a5f66f366a4df3a36b59c01906a0695 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 0b8ad3985e9e3b1825054fe1339cb41a5c59f4cd..98da02fce21736694803272e4e76e046080d2cd4 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;