From 1b6bab99c00c6f7ae20a95bbb17c394284f63900 Mon Sep 17 00:00:00 2001
From: armar-user <armar-user@kit.edu>
Date: Tue, 12 Nov 2019 10:00:50 +0100
Subject: [PATCH] HokuyoLaserUnit: Fix topics for each device object
---
.../HokuyoLaserUnit/HokuyoLaserUnit.cpp | 23 +++++++++++++++----
1 file changed, 19 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 4d0e0d710..061971032 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -69,8 +69,6 @@ void HokuyoLaserUnit::onInitComponent()
device.connected = false;
device.componentName = getName();
- device.scanTopic = topic;
- device.robotHealthTopic = robotHealthTopic;
}
catch (std::exception const& ex)
{
@@ -115,6 +113,9 @@ void HokuyoLaserUnit::onConnectComponent()
info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
device.lengthData.resize(lengthDataSize);
+ device.scanTopic = topic;
+ device.robotHealthTopic = robotHealthTopic;
+
connectedDevices.push_back(info);
ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", "
@@ -243,13 +244,27 @@ void HokuyoLaserScanDevice::run()
errorCounter = 0;
- scanTopic->reportSensorValues(ip, frame, scan, now);
+ if (scanTopic)
+ {
+ scanTopic->reportSensorValues(ip, frame, scan, now);
+ }
+ else
+ {
+ ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
+ }
IceUtil::Time time_topicSensor = TimeUtil::GetTime();
RobotHealthHeartbeatArgs args;
args.maximumCycleTimeWarningMS = 500;
args.maximumCycleTimeErrorMS = 800;
- robotHealthTopic->heartbeat(componentName + "_" + ip, args);
+ if (robotHealthTopic)
+ {
+ robotHealthTopic->heartbeat(componentName + "_" + ip, args);
+ }
+ else
+ {
+ ARMARX_WARNING << "No robot health topic available: IP: " << ip << ", Port: " << port;
+ }
IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
--
GitLab