diff --git a/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp b/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
index a286ec83a49a1d0fe34ef81330be13e56d14afae..111a542188d5d46aa6f5664aa5476d9c7dc6f88e 100644
--- a/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
+++ b/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
@@ -20,7 +20,7 @@
* GNU General Public License
*/
-#include <RobotAPI/components/HokyouLaserUnit/HokyouLaserUnit.h>
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
@@ -28,5 +28,5 @@
int main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::HokyouLaserUnit > (argc, argv, "HokyouLaserUnit");
+ return armarx::runSimpleComponentApp < armarx::HokuyoLaserUnit > (argc, argv, "HokuyoLaserUnit");
}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index a9169ff0ab28d87a122d8350e023b2e0c5330d09..fa3841d8f9a1e2772be93932d53e1f0804b05225 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -32,7 +32,7 @@ set(LIB_HEADERS
HapticObserver.h
InertialMeasurementUnit.h
InertialMeasurementUnitObserver.h
- HokuyoLaserUnitObserver.h
+ LaserScannerUnitObserver.h
TCPControlUnit.h
TCPControlUnitObserver.h
@@ -58,7 +58,7 @@ set(LIB_FILES
HapticObserver.cpp
InertialMeasurementUnit.cpp
InertialMeasurementUnitObserver.cpp
- HokuyoLaserUnitObserver.cpp
+ LaserScannerUnitObserver.cpp
TCPControlUnit.cpp
TCPControlUnitObserver.cpp
diff --git a/source/RobotAPI/components/units/HokuyoLaserUnitObserver.cpp b/source/RobotAPI/components/units/HokuyoLaserUnitObserver.cpp
deleted file mode 100644
index 6f07a399b6c6af0ff4ef4b7d25a73a887f59bb80..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/HokuyoLaserUnitObserver.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package
- * @author
- * @date
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#include "HokuyoLaserUnitObserver.h"
-
-
-#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
-#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
-#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-
-
-using namespace armarx;
-
-
-void HokuyoLaserUnitObserver::onInitObserver()
-{
- usingTopic(getProperty<std::string>("IMUTopicName").getValue());
-
- offerConditionCheck("updated", new ConditionCheckUpdated());
- offerConditionCheck("larger", new ConditionCheckLarger());
- offerConditionCheck("equals", new ConditionCheckEquals());
- offerConditionCheck("smaller", new ConditionCheckSmaller());
-
- offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
-
-
-void HokuyoLaserUnitObserver::onConnectObserver()
-{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
-}
-
-
-void HokuyoLaserUnitObserver::onExitObserver()
-{
- debugDrawerPrx->removePoseVisu("IMU", "orientation");
- debugDrawerPrx->removeLineVisu("IMU", "acceleration");
-}
-
-void HokuyoLaserUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
-{
- ScopedLock lock(dataMutex);
-
- TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
-
- Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
- Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
- Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
- QuaternionPtr orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
-
- if (!existsChannel(device))
- {
- offerChannel(device, "IMU data");
- }
-
- offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
- offerValue(device, "acceleration", acceleration);
- offerValue(device, "gyroscopeRotation", gyroscopeRotation);
- offerValue(device, "magneticRotation", magneticRotation);
- offerValue(device, "acceleration", acceleration);
- offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
- offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
-
- updateChannel(device);
-
- Eigen::Vector3f zero;
- zero.setZero();
-
- DrawColor color;
- color.r = 1;
- color.g = 1;
- color.b = 0;
- color.a = 0.5;
-
- Eigen::Vector3f ac = acceleration->toEigen();
- ac *= 10;
-
- debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
-
- PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
- debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
- debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
-}
-
-void HokuyoLaserUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
-{
- offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
- offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
- offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
- offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
-
-}
-
-
-PropertyDefinitionsPtr HokuyoLaserUnitObserver::createPropertyDefinitions()
-{
- return PropertyDefinitionsPtr(new HokuyoLaserUnitObserverPropertyDefinitions(getConfigIdentifier()));
-}
-
-
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ac2f21ac18610250b78a11716eabb19f86d07fe
--- /dev/null
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -0,0 +1,114 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package
+ * @author
+ * @date
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "LaserScannerUnitObserver.h"
+
+
+#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
+#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+
+
+using namespace armarx;
+
+
+void LaserScannerUnitObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("LaserScannerTopicName").getValue());
+
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+}
+
+
+
+void LaserScannerUnitObserver::onConnectObserver()
+{
+}
+
+
+void LaserScannerUnitObserver::onExitObserver()
+{
+}
+
+
+PropertyDefinitionsPtr LaserScannerUnitObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
+
+void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+ ScopedLock lock(dataMutex);
+
+ if (!existsChannel(device))
+ {
+ offerChannel(device, "laser scans");
+ }
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+ offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
+ // Calculate some statistics on the laser scan
+ float minDistance = FLT_MAX;
+ float minAngle = 0.0f;
+ float maxDistance = -FLT_MAX;
+ float maxAngle = 0.0f;
+ float distanceSum = 0.0f;
+ for (LaserScanStep const & step : scan)
+ {
+ distanceSum += step.distance;
+ if (step.distance < minDistance)
+ {
+ minDistance = step.distance;
+ minAngle = step.angle;
+ }
+ if (step.distance > maxDistance)
+ {
+ maxDistance = step.distance;
+ maxAngle = step.angle;
+ }
+ }
+
+ if (scan.size() > 0)
+ {
+ offerOrUpdateDataField(device, "minDistance", minDistance, "minimal distance in scan");
+ offerOrUpdateDataField(device, "minAngle", minAngle, "angle with minimal distance in scan");
+ offerOrUpdateDataField(device, "maxDistance", maxDistance, "maximal distance in scan");
+ offerOrUpdateDataField(device, "maxAngle", maxAngle, "angle with maximal distance in scan");
+ float averageDistance = distanceSum / scan.size();
+ offerOrUpdateDataField(device, "averageDistance", averageDistance, "average distance in scan");
+ }
+
+ updateChannel(device);
+}
+
+
diff --git a/source/RobotAPI/components/units/HokuyoLaserUnitObserver.h b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
similarity index 59%
rename from source/RobotAPI/components/units/HokuyoLaserUnitObserver.h
rename to source/RobotAPI/components/units/LaserScannerUnitObserver.h
index df12253beef759522168553bbb0bce9c9c0755e1..d982ce5256b3c280f79b2a47c23cc17eaf25d368 100644
--- a/source/RobotAPI/components/units/HokuyoLaserUnitObserver.h
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
@@ -22,10 +22,10 @@
* GNU General Public License
*/
-#ifndef _ARMARX_ROBOTAPI_IMU_OBSERVER_H
-#define _ARMARX_ROBOTAPI_IMU_OBSERVER_H
+#ifndef _ARMARX_ROBOTAPI_LASER_SCANNER_UNIT_OBSERVER_H
+#define _ARMARX_ROBOTAPI_LASER_SCANNER_UNIT_OBSERVER_H
-#include <RobotAPI/interface/units/HokuyoLaserUnit.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -34,59 +34,53 @@
namespace armarx
{
/**
- * \class HokuyoLaserUnitObserverPropertyDefinitions
+ * \class LaserScannerUnitObserverPropertyDefinitions
* \brief
*/
- class HokuyoLaserUnitObserverPropertyDefinitions:
+ class LaserScannerUnitObserverPropertyDefinitions:
public ObserverPropertyDefinitions
{
public:
- HokuyoLaserUnitObserverPropertyDefinitions(std::string prefix):
+ LaserScannerUnitObserverPropertyDefinitions(std::string prefix):
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
}
};
/**
- * \class HokuyoLaserUnitObserver
+ * \class LaserScannerUnitObserver
* \ingroup RobotAPI-SensorActorUnits-observers
- * \brief Observer monitoring @IMU sensor values
+ * \brief Observer monitoring laser scanner values
*
- * The HokuyoLaserUnitObserver monitors @IMU sensor values published by HokuyoLaserUnit-implementations and offers condition checks on these values.
- * Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
+ * The LaserScannerUnitObserver monitors laser scanner values published by LaserScannerUnit-implementations.
*/
- class HokuyoLaserUnitObserver :
+ class LaserScannerUnitObserver :
virtual public Observer,
- virtual public HokuyoLaserUnitObserverInterface
+ virtual public LaserScannerUnitObserverInterface
{
public:
- HokuyoLaserUnitObserver() {}
+ LaserScannerUnitObserver() {}
virtual std::string getDefaultName() const
{
- return "HokuyoLaserUnitObserver";
+ return "LaserScannerUnitObserver";
}
virtual void onInitObserver();
virtual void onConnectObserver();
virtual void onExitObserver();
-
- void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current());
-
/**
* @see PropertyUser::createPropertyDefinitions()
*/
virtual PropertyDefinitionsPtr createPropertyDefinitions();
+ void reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan,
+ const TimestampBasePtr& timestamp, const Ice::Current& c) override;
private:
Mutex dataMutex;
- DebugDrawerInterfacePrx debugDrawerPrx;
-
- void offerValue(std::string device, std::string fieldName, Vector3Ptr vec);
};
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
index 0216cfda88316dc70ca7500ef5a4bf4a4aab72b6..581618323d926db09e1559e695df1345ed865190 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
@@ -25,7 +25,7 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/HokuyoLaserUnit/HokuyoLaserUnit.h>
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
#include <iostream>
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index f581f92d9aa0395e248966c04482c1a0dc702616..05f94f9bd5f6e0c55eb68b2e920acb88d7f9be0b 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -23,7 +23,7 @@ set(SLICE_FILES
units/ForceTorqueUnit.ice
units/InertialMeasurementUnit.ice
- units/HokuyoLaserUnit.ice
+ units/LaserScannerUnit.ice
units/HandUnitInterface.ice
units/HapticUnit.ice
units/WeissHapticUnit.ice
diff --git a/source/RobotAPI/interface/units/HokuyoLaserUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
similarity index 54%
rename from source/RobotAPI/interface/units/HokuyoLaserUnit.ice
rename to source/RobotAPI/interface/units/LaserScannerUnit.ice
index 5b96ae5160ee6ba4ce437eb8f9a127d010f81703..8614ca447d383f342128ce4848f859b61a9d1dae 100644
--- a/source/RobotAPI/interface/units/HokuyoLaserUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -22,8 +22,8 @@
* GNU General Public License
*/
-#ifndef _ARMARX_ROBOTAPI_UNITS_HOKYOULASERUNIT_SLICE_
-#define _ARMARX_ROBOTAPI_UNITS_HOKYOULASERUNIT_SLICE_
+#ifndef _ARMARX_ROBOTAPI_UNITS_LASER_SCANNER_UNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_LASER_SCANNER_UNIT_SLICE_
#include <RobotAPI/interface/units/UnitInterface.ice>
@@ -41,39 +41,27 @@
module armarx
{
/**
- * Struct IMUData with which IMU sensor data is represented. It incorporates following entries:
- * @param orientationQuaternion Orientation in quaternion.
- * @param magneticRotation Magnetic rotation.
- * @param gyroscopeRotation Rotation of gyroscope.
- * @param acceleration Acceleration of IMU sensor device.
+ * Struct LaserScanStep with which a single scan step is represented. It incorporates following entries:
+ * @param angle Angle in which direction a distance was measured [rad].
+ * @param distance The measured distance [mm].
**/
- struct LaserData {
- Ice::Float angle;
- Ice::Float distance;
+ struct LaserScanStep {
+ float angle;
+ float distance;
};
- /**
- * Implements an interface to an HokuyoLaserUnit.
- **/
- interface HokuyoLaserUnitInterface extends armarx::SensorActorUnitInterface
+
+ sequence<LaserScanStep> LaserScan;
+
+ interface LaserScannerUnitInterface extends armarx::SensorActorUnitInterface
{
};
- /**
- * Implements an interface to an HokuyoLaserUnitListener.
- **/
- interface HokuyoLaserUnitListener
+
+ interface LaserScannerUnitListener
{
- /**
- * reportSensorValues reports the IMU sensor values from a given sensor device.
- * @param device Name of IMU sensor device.
- * @param values IMU sensor data.
- * @param timestamp Timestamp of the measurement.
- **/
- void reportSensorValues(string device, string name, IMUData values, TimestampBase timestamp);
+ void reportSensorValues(string device, string name, LaserScan values, TimestampBase timestamp);
};
- /**
- * Implements an interface to an HokuyoLaserUnitObserver.
- **/
- interface HokuyoLaserUnitObserverInterface extends ObserverInterface, HokuyoLaserUnitListener
+
+ interface LaserScannerUnitObserverInterface extends ObserverInterface, LaserScannerUnitListener
{
};