Skip to content
Snippets Groups Projects
Commit a38cc15f authored by Fabian Reister's avatar Fabian Reister Committed by ARMAR-6 User
Browse files

Change topic to proxy connection for Hokuyo unit

parent 578912f5
No related branches found
No related tags found
1 merge request!372Changes from ARMAR-6 Vision PC
......@@ -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";
}
}
......
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment