Hokuyo: Rename interface to LaserScannerUnit

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");
 }

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
 

source/RobotAPI/components/units/HokuyoLaserUnitObserver.cpp → source/RobotAPI/components/units/LaserScannerUnitObserver.cpp

@@ -21,7 +21,7 @@
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
-#include "HokuyoLaserUnitObserver.h"
+#include "LaserScannerUnitObserver.h"
 
 
 #include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
@@ -38,90 +38,77 @@
 using namespace armarx;
 
 
-void HokuyoLaserUnitObserver::onInitObserver()
+void LaserScannerUnitObserver::onInitObserver()
 {
-    usingTopic(getProperty<std::string>("IMUTopicName").getValue());
+    usingTopic(getProperty<std::string>("LaserScannerTopicName").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()
+void LaserScannerUnitObserver::onConnectObserver()
 {
-    debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
 }
 
 
-void HokuyoLaserUnitObserver::onExitObserver()
+void LaserScannerUnitObserver::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);
+PropertyDefinitionsPtr LaserScannerUnitObserver::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
 
-    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));
+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, "IMU data");
+        offerChannel(device, "laser scans");
     }
 
-    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");
+    TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
     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");
-
-}
+    // 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");
+    }
 
-PropertyDefinitionsPtr HokuyoLaserUnitObserver::createPropertyDefinitions()
-{
-    return PropertyDefinitionsPtr(new HokuyoLaserUnitObserverPropertyDefinitions(getConfigIdentifier()));
+    updateChannel(device);
 }
 
 

source/RobotAPI/components/units/HokuyoLaserUnitObserver.h → 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);
     };
 }
 

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>
 

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

source/RobotAPI/interface/units/HokuyoLaserUnit.ice → 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
     {
     };