Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

data/RobotAPI/robots/Armar3/ArmarIII.xml

@@ -70,9 +70,42 @@
 		<CollisionModel>
 			<File type="Inventor">convexModel/platform.iv</File>
 		</CollisionModel>
+		<Child name="Laser Scanner Front"/>
+		<Child name="Laser Scanner Back Left"/>
+		<Child name="Laser Scanner Back Right"/>
 		<Child name="Hip Pitch"/>
 	</RobotNode>
 
+	<RobotNode name="Laser Scanner Front">
+		<Transform>
+			<DH a="330" d="100" theta="90" alpha="0" units="degree"/>
+			<DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
+		</Transform>
+		<Visualization enable="true">
+			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
+		</Visualization>
+	</RobotNode>
+
+	<RobotNode name="Laser Scanner Back Left">
+		<Transform>
+			<DH a="320" d="100" theta="210" alpha="0" units="degree"/>
+			<DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
+		</Transform>
+		<Visualization enable="true">
+			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
+		</Visualization>		
+	</RobotNode>
+
+	<RobotNode name="Laser Scanner Back Right">
+		<Transform>
+			<DH a="320" d="100" theta="340" alpha="0" units="degree"/>
+			<DH a="0" d="0" theta="-90" alpha="0" units="degree"/>
+		</Transform>
+		<Visualization enable="true">
+			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
+		</Visualization>
+	</RobotNode>
+
 	<RobotNode name="Hip Pitch">
 			<Transform>
 				<Translation x="0" y="158" z="890" units='mm'/>

etc/doxygen/pages/GuiPlugins.dox

@@ -18,4 +18,6 @@ The following Gui Plugins are available:
 \subpage RobotAPI-GuiPlugins-ViewSelection
 
 \subpage ArmarXGui-GuiPlugins-DebugDrawerViewer
+
+\subpage ArmarXGui-GuiPlugins-LaserScannerPlugin
 */

scenarios/Armar3Replay/Armar3Replay.scx

 <?xml version="1.0" encoding="utf-8"?>
-<scenario name="Armar3Replay" lastChange="2017-03-01.15:34:31" creation="2017-02-14.19:27:46" globalConfigName="./config/global.cfg" package="RobotAPI">
-	<application name="SystemObserver" instance="" package="ArmarXCore"/>
-	<application name="DebugObserver" instance="" package="ArmarXCore"/>
-	<application name="KinematicUnitObserver" instance="" package="RobotAPI"/>
-	<application name="PlatformUnitObserver" instance="" package="RobotAPI"/>
-	<application name="ForceTorqueObserver" instance="" package="RobotAPI"/>
-	<application name="KinematicUnitSimulation" instance="" package="RobotAPI"/>
-	<application name="RobotStateComponent" instance="" package="RobotAPI"/>
-	<application name="InertialMeasurementUnitObserverApp" instance="" package="RobotAPI"/>
-	<application name="ConditionHandler" instance="" package="ArmarXCore"/>
+<scenario name="Armar3Replay" creation="2017-02-14.19:27:46" lastChange="2017-02-14.19:27:46" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="SystemObserver" instance="" package="ArmarXCore" enabled="true"/>
+	<application name="DebugObserver" instance="" package="ArmarXCore" enabled="true"/>
+	<application name="KinematicUnitObserver" instance="" package="RobotAPI" enabled="true"/>
+	<application name="PlatformUnitObserver" instance="" package="RobotAPI" enabled="true"/>
+	<application name="ForceTorqueObserver" instance="" package="RobotAPI" enabled="true"/>
+	<application name="KinematicUnitSimulation" instance="" package="RobotAPI" enabled="true"/>
+	<application name="RobotStateComponent" instance="" package="RobotAPI" enabled="true"/>
+	<application name="InertialMeasurementUnitObserverApp" instance="" package="RobotAPI" enabled="true"/>
+	<application name="ConditionHandler" instance="" package="ArmarXCore" enabled="true"/>
 </scenario>
 

scenarios/LaserScannerTest/LaserScannerTest.scx

+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="LaserScannerTest" creation="2017-03-07.16:12:47" lastChange="2017-03-07.16:12:47" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="HokuyoLaserUnitApp" instance="" package="RobotAPI" enabled="true"/>
+	<application name="LaserScannerUnitObserverApp" instance="" package="RobotAPI" enabled="true"/>
+</scenario>
+

scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg

+# ==================================================================
+# HokuyoLaserUnitApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages:  List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName:  Application name
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath:  Path for cache files
+#  Attributes:
+#  - Default:            ${HOME}/.armarx/mongo/.cache
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.Config:  Comma-separated list of configuration files 
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath:  Semicolon-separated search list for data files
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages:  List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig:  Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+#  Attributes:
+#  - Default:            ./config/dependencies.cfg
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging:  Turn logging off in whole application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.EnableProfiling:  Enable profiling of CPU load produced by this application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.EnableProfiling = 0
+
+
+# ArmarX.HokuyoLaserUnit.AngleOffset:  Offset is applied the raw angles before reporting them
+#  Attributes:
+#  - Default:            -2.3561945
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.HokuyoLaserUnit.AngleOffset = 0
+
+
+# ArmarX.HokuyoLaserUnit.Devices:  List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.HokuyoLaserUnit.Devices = "192.168.0.11,10940,Laser Scanner A"
+
+
+# ArmarX.HokuyoLaserUnit.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.EnableProfiling = 0
+
+
+# ArmarX.HokuyoLaserUnit.LaserScannerTopicName:  Name of the laser scan topic.
+#  Attributes:
+#  - Default:            LaserScans
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.LaserScannerTopicName = LaserScans
+
+
+# ArmarX.HokuyoLaserUnit.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HokuyoLaserUnit.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.ObjectName = ""
+
+
+# ArmarX.HokuyoLaserUnit.UpdatePeriod:  Update period for laser scans
+#  Attributes:
+#  - Default:            25
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.UpdatePeriod = 25
+
+
+# ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RedirectStdout = 1
+
+
+# ArmarX.RemoteHandlesDeletionTimeout:  The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+#  Attributes:
+#  - Default:            3000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash:  If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.TopicSuffix:  Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer:  Enable using a global Timeserver (e.g. from ArmarXSimulator)
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity:  Global logging level for whole application
+#  Attributes:
+#  - Default:            Info
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Verbosity = Info
+
+
+
+
+

scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg

+# ==================================================================
+# LaserScannerUnitObserverApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages:  List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName:  Application name
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath:  Path for cache files
+#  Attributes:
+#  - Default:            ${HOME}/.armarx/mongo/.cache
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.Config:  Comma-separated list of configuration files 
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath:  Semicolon-separated search list for data files
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages:  List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig:  Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+#  Attributes:
+#  - Default:            ./config/dependencies.cfg
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging:  Turn logging off in whole application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.EnableProfiling:  Enable profiling of CPU load produced by this application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.EnableProfiling = 0
+
+
+# ArmarX.LaserScannerUnitObserver.CreateUpdateFrequenciesChannel:  If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.LaserScannerUnitObserver.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.EnableProfiling = 0
+
+
+# ArmarX.LaserScannerUnitObserver.LaserScannerTopicName:  Name of the laser scan topic.
+#  Attributes:
+#  - Default:            LaserScans
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.LaserScannerTopicName = LaserScans
+
+
+# ArmarX.LaserScannerUnitObserver.MaxHistoryRecordFrequency:  The Observer history is written with this maximum frequency. Everything faster is being skipped.
+#  Attributes:
+#  - Default:            50
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.LaserScannerUnitObserver.MaxHistorySize:  Maximum number of entries in the Observer history
+#  Attributes:
+#  - Default:            5000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.MaxHistorySize = 5000
+
+
+# ArmarX.LaserScannerUnitObserver.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.LaserScannerUnitObserver.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.ObjectName = ""
+
+
+# ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RedirectStdout = 1
+
+
+# ArmarX.RemoteHandlesDeletionTimeout:  The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+#  Attributes:
+#  - Default:            3000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash:  If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.TopicSuffix:  Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer:  Enable using a global Timeserver (e.g. from ArmarXSimulator)
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity:  Global logging level for whole application
+#  Attributes:
+#  - Default:            Info
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Verbosity = Info
+
+
+
+
+

scenarios/LaserScannerTest/config/global.cfg

+# ==================================================================
+# Global Config from Scenario LaserScannerTest
+# ==================================================================
+
+
+
+

source/RobotAPI/applications/CMakeLists.txt

@@ -26,4 +26,7 @@ add_subdirectory(OrientedTactileSensorUnit)
 add_subdirectory(OrientedTactileSensorUnitObserver)
 
 add_subdirectory(OptoForceUnit)
-add_subdirectory(OptoForceUnitObserver)
\ No newline at end of file
+add_subdirectory(OptoForceUnitObserver)
+
+add_subdirectory(HokuyoLaserUnit)
+add_subdirectory(LaserScannerUnitObserver)

source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt

+armarx_component_set_name("HokuyoLaserUnitApp")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+find_package(HokuyoLaserScannerDriver QUIET)
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces
+    ArmarXCore
+    HokuyoLaserUnit
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")

source/RobotAPI/applications/HokuyoLaserUnit/main.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * 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    RobotAPI::application::HokyouLaserUnit
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    return armarx::runSimpleComponentApp < armarx::HokuyoLaserUnit > (argc, argv, "HokuyoLaserUnit");
+}

source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt

+armarx_component_set_name("LaserScannerUnitObserverApp")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces
+    ArmarXCore
+    RobotAPIUnits
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")

source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * 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    RobotAPI::application::LaserScannerUnitObserver
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/components/units/LaserScannerUnitObserver.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    return armarx::runSimpleComponentApp < armarx::LaserScannerUnitObserver > (argc, argv, "LaserScannerUnitObserver");
+}

source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp

@@ -25,6 +25,7 @@
 
 #include "DebugDrawerComponent.h"
 
+#include <RobotAPI/libraries/core/math/ColorUtils.h>
 
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
@@ -621,8 +622,16 @@ namespace armarx
             coordinateValues[2 * i].setValue(d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
             coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x, d.lineSet.points[2 * i + 1].y, d.lineSet.points[2 * i + 1].z);
 
-            VirtualRobot::VisualizationFactory::Color c = visualizationColorMap.getColor(d.lineSet.intensities[i]);
-            colorValues[i].setValue(c.r, c.g, c.b);
+            if (d.lineSet.useHeatMap)
+            {
+                auto c = colorutils::HeatMapRGBAColor(d.lineSet.intensities[i]);
+                colorValues[i].setValue(c.r, c.g, c.b);
+            }
+            else
+            {
+                VirtualRobot::VisualizationFactory::Color c = visualizationColorMap.getColor(d.lineSet.intensities[i]);
+                colorValues[i].setValue(c.r, c.g, c.b);
+            }
         }
 
         coordinateNode->point.setValuesPointer(d.lineSet.points.size(), coordinateValues);

source/RobotAPI/components/units/CMakeLists.txt

@@ -34,6 +34,7 @@ set(LIB_HEADERS
     InertialMeasurementUnitObserver.h
     OptoForceUnitObserver.h
     OrientedTactileSensorUnitObserver.h
+    LaserScannerUnitObserver.h
     TCPControlUnit.h
     TCPControlUnitObserver.h
 
@@ -61,6 +62,7 @@ set(LIB_FILES
     InertialMeasurementUnitObserver.cpp
     OptoForceUnitObserver.cpp
     OrientedTactileSensorUnitObserver.cpp
+    LaserScannerUnitObserver.cpp
     TCPControlUnit.cpp
     TCPControlUnitObserver.cpp
 

source/RobotAPI/components/units/LaserScannerUnitObserver.cpp

+/*
+ * 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/>.
+ *
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @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);
+}
+
+

source/RobotAPI/components/units/LaserScannerUnitObserver.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-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    RobotAPI::units
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LASER_SCANNER_UNIT_OBSERVER_H
+#define _ARMARX_ROBOTAPI_LASER_SCANNER_UNIT_OBSERVER_H
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx
+{
+    /**
+     * \class LaserScannerUnitObserverPropertyDefinitions
+     * \brief
+     */
+    class LaserScannerUnitObserverPropertyDefinitions:
+        public ObserverPropertyDefinitions
+    {
+    public:
+        LaserScannerUnitObserverPropertyDefinitions(std::string prefix):
+            ObserverPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+        }
+    };
+
+
+    /**
+     * \class LaserScannerUnitObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring laser scanner values
+     *
+     * The LaserScannerUnitObserver monitors laser scanner values published by LaserScannerUnit-implementations.
+     */
+    class LaserScannerUnitObserver :
+        virtual public Observer,
+        virtual public LaserScannerUnitObserverInterface
+    {
+    public:
+        LaserScannerUnitObserver() {}
+
+        virtual std::string getDefaultName() const
+        {
+            return "LaserScannerUnitObserver";
+        }
+        virtual void onInitObserver();
+        virtual void onConnectObserver();
+        virtual void onExitObserver();
+        /**
+         * @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;
+
+    };
+}
+
+#endif

source/RobotAPI/drivers/CMakeLists.txt

@@ -2,4 +2,4 @@ add_subdirectory(WeissHapticSensor)
 add_subdirectory(XsensIMU)
 add_subdirectory(OptoForceUnit)
 add_subdirectory(OrientedTactileSensor)
-
+add_subdirectory(HokuyoLaserUnit)

source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt

+armarx_component_set_name("HokuyoLaserUnit")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+find_package(HokuyoLaserScannerDriver QUIET)
+armarx_build_if(HokuyoLaserScannerDriver_FOUND "Hokuyo laser scanner driver not found")
+
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants
+    RobotAPICore RobotAPIUnits
+    HokuyoLaserScannerDriver)
+
+set(SOURCES
+./HokuyoLaserUnit.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./HokuyoLaserUnit.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+add_subdirectory(test)

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * 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    RobotAPI::ArmarXObjects::HokuyoLaserUnit
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "HokuyoLaserUnit.h"
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+#include <boost/algorithm/string/split.hpp>
+#include <urg_utils.h>
+
+using namespace armarx;
+
+
+void HokuyoLaserUnit::onInitComponent()
+{
+    topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
+    offeringTopic(topicName);
+    ARMARX_INFO << "Going to report on topic " << topicName;
+    updatePeriod = getProperty<int>("UpdatePeriod").getValue();
+    angleOffset = getProperty<float>("AngleOffset").getValue();
+
+    std::string deviceStrings = getProperty<std::string>("Devices").getValue();
+    std::vector<std::string> splitDeviceStrings;
+    boost::split(splitDeviceStrings, deviceStrings, boost::is_any_of(";"));
+    devices.clear();
+    devices.reserve(splitDeviceStrings.size());
+    for (std::string const & deviceString : splitDeviceStrings)
+    {
+        std::vector<std::string> deviceInfo;
+        boost::split(deviceInfo, deviceString, boost::is_any_of(","));
+        if (deviceInfo.size() != 3)
+        {
+            ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
+                           << " (split size: " << deviceInfo.size() << ")";
+            continue;
+        }
+
+        try
+        {
+            HokuyoLaserScanDevice device;
+            device.ip = deviceInfo[0];
+            device.port = std::stoi(deviceInfo[1]);
+            device.frame = deviceInfo[2];
+            device.connected = false;
+            devices.push_back(device);
+        }
+        catch (std::exception const& ex)
+        {
+            ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString
+                           << " (port is " << deviceInfo[1] << ") : " << ex.what();
+            continue;
+        }
+    }
+}
+
+
+void HokuyoLaserUnit::onConnectComponent()
+{
+    topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
+
+    if (task)
+    {
+        task->stop();
+    }
+
+    connectedDevices.clear();
+    for (HokuyoLaserScanDevice & device : devices)
+    {
+        ARMARX_INFO << "Connecting to " << device.ip << ":" << device.port;
+        int ret = urg_open(&device.urg, URG_ETHERNET, device.ip.c_str(), device.port);
+        device.connected = (ret == 0);
+        if (!device.connected)
+        {
+            ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
+                           << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+            continue;
+        }
+
+        LaserScannerInfo info;
+        info.device = device.ip;
+        info.frame = device.frame;
+        int minStep = 0, maxStep = 0;
+        urg_step_min_max(&device.urg, &minStep, &maxStep);
+        info.minAngle = urg_step2rad(&device.urg, minStep);
+        info.maxAngle = urg_step2rad(&device.urg, maxStep);
+
+
+        int lengthDataSize = urg_max_data_size(&device.urg);
+        info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
+        device.lengthData.resize(lengthDataSize);
+
+        connectedDevices.push_back(info);
+
+        ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", "
+                    << info.minAngle << ", " << info.maxAngle << ", " << info.stepSize;
+    }
+
+    task = new PeriodicTask<HokuyoLaserUnit>(this, &HokuyoLaserUnit::updateScanData, updatePeriod, false, "HokuyoLaserScanUpdate");
+    task->start();
+}
+
+
+void HokuyoLaserUnit::onDisconnectComponent()
+{
+    if (task)
+    {
+        task->stop();
+    }
+
+    for (HokuyoLaserScanDevice & device : devices)
+    {
+        if (device.connected)
+        {
+            urg_close(&device.urg);
+            device.connected = false;
+        }
+    }
+}
+
+
+void HokuyoLaserUnit::onExitComponent()
+{
+
+}
+
+armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
+{
+    return armarx::PropertyDefinitionsPtr(new HokuyoLaserUnitPropertyDefinitions(
+            getConfigIdentifier()));
+}
+
+std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current& c) const
+{
+    return topicName;
+}
+
+LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current& c) const
+{
+    return connectedDevices;
+}
+
+void HokuyoLaserUnit::updateScanData()
+{
+    LaserScan scan;
+    TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
+    for (HokuyoLaserScanDevice & device : devices)
+    {
+        if (device.connected)
+        {
+            int ret = urg_start_measurement(&device.urg, URG_DISTANCE, 1, 0);
+            if (ret != 0)
+            {
+                ARMARX_WARNING << deactivateSpam(1) << "Could not start measurement for laser scanner (IP: "
+                               << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+                continue;
+            }
+
+            int lengthDataSize = urg_get_distance(&device.urg, device.lengthData.data(), nullptr);
+            if (lengthDataSize < 0)
+            {
+                ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
+                               << device.ip << ", Port: " << device.port << ", Error: " << lengthDataSize << ")";
+                continue;
+            }
+
+            scan.clear();
+            scan.reserve(lengthDataSize);
+            for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
+            {
+                LaserScanStep step;
+                long distance = device.lengthData[stepIndex];
+                // TODO: Extract the min and max valid value for distance into parameters?
+                if (distance >= 21 && distance <= 30000)
+                {
+                    step.angle = angleOffset + (float)urg_index2rad(&device.urg, stepIndex); // Convert steps to rad
+                    step.distance = (float)distance; // Data is already in mm
+                    scan.push_back(step);
+                }
+            }
+
+            topic->reportSensorValues(device.ip, device.frame, scan, now);
+        }
+    }
+}
+

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h

+/*
+ * This file is part of ArmarX.
+ *
+ * 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    RobotAPI::ArmarXObjects::HokuyoLaserUnit
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_HokuyoLaserUnit_H
+#define _ARMARX_COMPONENT_RobotAPI_HokuyoLaserUnit_H
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+#include <urg_sensor.h>
+#include <vector>
+
+namespace armarx
+{
+    /**
+     * @class HokuyoLaserUnitPropertyDefinitions
+     * @brief
+     */
+    class HokuyoLaserUnitPropertyDefinitions:
+        public armarx::ComponentPropertyDefinitions
+    {
+    public:
+        HokuyoLaserUnitPropertyDefinitions(std::string prefix):
+            armarx::ComponentPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+            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;...'");
+        }
+    };
+
+    struct HokuyoLaserScanDevice
+    {
+        std::string ip;
+        int port = 0;
+        std::string frame;
+        bool connected = false;
+        urg_t urg;
+        std::vector<long> lengthData;
+    };
+
+    /**
+     * @defgroup Component-HokuyoLaserUnit HokuyoLaserUnit
+     * @ingroup RobotAPI-Components
+     * A description of the component HokuyoLaserUnit.
+     *
+     * @class HokuyoLaserUnit
+     * @ingroup Component-HokuyoLaserUnit
+     * @brief Brief description of class HokuyoLaserUnit.
+     *
+     * Detailed description of class HokuyoLaserUnit.
+     */
+    class HokuyoLaserUnit :
+        virtual public armarx::LaserScannerUnitInterface,
+        virtual public armarx::SensorActorUnit
+    {
+    public:
+        /**
+         * @see armarx::ManagedIceObject::getDefaultName()
+         */
+        virtual std::string getDefaultName() const
+        {
+            return "HokuyoLaserUnit";
+        }
+
+    protected:
+        /**
+         * @see armarx::ManagedIceObject::onInitComponent()
+         */
+        virtual void onInitComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onConnectComponent()
+         */
+        virtual void onConnectComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onDisconnectComponent()
+         */
+        virtual void onDisconnectComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onExitComponent()
+         */
+        virtual void onExitComponent();
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
+
+        std::string getReportTopicName(const Ice::Current& c) const override;
+
+        LaserScannerInfoSeq getConnectedDevices(const Ice::Current& c) const override;
+
+    private:
+        void updateScanData();
+
+    private:
+        std::string topicName;
+        LaserScannerUnitListenerPrx topic;
+        int updatePeriod = 25;
+        float angleOffset = 0.0f;
+        std::vector<HokuyoLaserScanDevice> devices;
+        PeriodicTask<HokuyoLaserUnit>::pointer_type task;
+        LaserScannerInfoSeq connectedDevices;
+    };
+}
+
+#endif