diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp index 4ee7f8ec607a5754a93a30c1a1cb49c040932b69..69c6d9bcd13c342d895909bc2ce1de353f6e9268 100644 --- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp +++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp @@ -32,6 +32,8 @@ using namespace armarx; void HokuyoLaserUnit::onInitComponent() { + offeringTopic(getProperty<std::string>("RobotHealthTopicName").getValue()); + topicName = getProperty<std::string>("LaserScannerTopicName").getValue(); offeringTopic(topicName); ARMARX_INFO << "Going to report on topic " << topicName; @@ -75,6 +77,7 @@ void HokuyoLaserUnit::onInitComponent() void HokuyoLaserUnit::onConnectComponent() { + robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("LaserScannerTopicName").getValue()); topic = getTopic<LaserScannerUnitListenerPrx>(topicName); if (task) @@ -199,6 +202,7 @@ void HokuyoLaserUnit::updateScanData() } topic->reportSensorValues(device.ip, device.frame, scan, now); + robotHealthTopic->heartbeat(getName() + "_" + device.ip); } } } diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h index 78feddf97de34cf24143eddec3c311a81c5c6a69..40f9d4861db9abcfc9043dd26f6d454edc642fea 100644 --- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h +++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h @@ -26,6 +26,7 @@ #include <ArmarXCore/core/services/tasks/PeriodicTask.h> #include <RobotAPI/components/units/SensorActorUnit.h> #include <RobotAPI/interface/units/LaserScannerUnit.h> +#include <RobotAPI/interface/components/RobotHealthInterface.h> #include <HokuyoLaserScannerDriver/urg_sensor.h> #include <vector> @@ -47,6 +48,7 @@ namespace armarx defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans"); defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them"); defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'"); + defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic"); } }; @@ -125,6 +127,7 @@ namespace armarx std::vector<HokuyoLaserScanDevice> devices; PeriodicTask<HokuyoLaserUnit>::pointer_type task; LaserScannerInfoSeq connectedDevices; + RobotHealthInterfacePrx robotHealthTopic; }; }