diff --git a/CMakeLists.txt b/CMakeLists.txt
index 610f43fb47a751b5975e98819958cf898ee36371..f02294b9825d19a33092ea38aa37f2a311d133c2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,7 +14,7 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui "OPTIONAL")
-set(ArmarX_Simox_VERSION 2.3.42)
+set(ArmarX_Simox_VERSION 2.3.48)
option(REQUIRE_SIMOX "If enabled the Simox dependency is a required dependency" TRUE)
if(REQUIRE_SIMOX)
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII.xml b/data/RobotAPI/robots/Armar3/ArmarIII.xml
index bea24c5a2040ef6118e3d5b8ffc5945b49d85d7a..b1b1d8e283c920337e721777b8c6061ee40e0b54 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII.xml
+++ b/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'/>
diff --git a/data/RobotAPI/sensors/OptoForceCalibration.xml b/data/RobotAPI/sensors/OptoForceCalibration.xml
index b863b0de893b1dc467e5ff2b0418c163539badc5..791d390e5ac7dad89f11af20b29f2a82c86d05ad 100644
--- a/data/RobotAPI/sensors/OptoForceCalibration.xml
+++ b/data/RobotAPI/sensors/OptoForceCalibration.xml
@@ -1,11 +1,23 @@
<Calibration>
<Daq serialNumber="KIT0A003">
- <Sensor nominalCapacity="10" counts_at_nc="3870" name="DSE0A102"/>
- <Sensor nominalCapacity="10" counts_at_nc="4001" name="DSE0A103"/>
- <Sensor nominalCapacity="10" counts_at_nc="4021" name="DSE0A104"/>
- <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A108"/>
+ <Sensor nominalCapacity="10" counts_at_nc="3870" name="DSE0A102" offsetX="0" offsetY="0" offsetZ="0" />
+ <Sensor nominalCapacity="10" counts_at_nc="4001" name="DSE0A103" offsetX="0" offsetY="0" offsetZ="0" />
+ <Sensor nominalCapacity="10" counts_at_nc="4021" name="DSE0A104" offsetX="0" offsetY="0" offsetZ="0" />
+ <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A108" offsetX="0" offsetY="0" offsetZ="0" />
</Daq>
<Daq serialNumber="DSE0A114">
- <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A114" />
+ <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A114" offsetX="0" offsetY="0" offsetZ="0" />
+ </Daq>
+ <Daq serialNumber="KIT0A001">
+ <Sensor nominalCapacity="100" counts_at_nc="16091" name="MSE0A052" offsetX="1.4" offsetY="4.5" offsetZ="3.2" />
+ <Sensor nominalCapacity="100" counts_at_nc="16279" name="MSE0A055" offsetX="-0.4" offsetY="3.3" offsetZ="1.2" />
+ <Sensor nominalCapacity="100" counts_at_nc="16090" name="MSE0A056" offsetX="-0.8" offsetY="0.6" offsetZ="-0.8" />
+ <Sensor nominalCapacity="100" counts_at_nc="15888" name="MSE0A057" offsetX="0.2" offsetY="-4.1" offsetZ="-2.8" />
+ </Daq>
+ <Daq serialNumber="KIT0A002">
+ <Sensor nominalCapacity="100" counts_at_nc="16021" name="MSE0A058" offsetX="-0.1" offsetY="-1.1" offsetZ="-0.5" />
+ <Sensor nominalCapacity="100" counts_at_nc="16061" name="MSE0A059" offsetX="-1.1" offsetY="0.1" offsetZ="-0.5" />
+ <Sensor nominalCapacity="100" counts_at_nc="15840" name="MSE0A060" offsetX="-1.5" offsetY="-9.8" offsetZ="2.2" />
+ <Sensor nominalCapacity="100" counts_at_nc="0" name="dummy" enabled="false" offsetX="0" offsetY="0" offsetZ="0" />
</Daq>
</Calibration>
diff --git a/etc/doxygen/pages/GuiPlugins.dox b/etc/doxygen/pages/GuiPlugins.dox
index 8f8fe7c3b4285b1cb05e9afc6fc2dad92a502bd2..5bde34684bc76514eb49696b35b4e2e0ce0946a6 100644
--- a/etc/doxygen/pages/GuiPlugins.dox
+++ b/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
*/
diff --git a/scenarios/Armar3Replay/Armar3Replay.scx b/scenarios/Armar3Replay/Armar3Replay.scx
index 5d05f1e1ff57737f4a15bf97ada9840e38fc56a7..f2d7bbba96fff5299a19852446f8ea24f835cd78 100644
--- a/scenarios/Armar3Replay/Armar3Replay.scx
+++ b/scenarios/Armar3Replay/Armar3Replay.scx
@@ -1,13 +1,13 @@
<?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>
diff --git a/scenarios/LaserScannerTest/LaserScannerTest.scx b/scenarios/LaserScannerTest/LaserScannerTest.scx
new file mode 100644
index 0000000000000000000000000000000000000000..585406ce7b9f493cb159af1da819db3a56a03a17
--- /dev/null
+++ b/scenarios/LaserScannerTest/LaserScannerTest.scx
@@ -0,0 +1,6 @@
+<?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>
+
diff --git a/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..effbf0059a3f5aee7da7b6aa9b5d3b7b46c9194c
--- /dev/null
+++ b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
@@ -0,0 +1,183 @@
+# ==================================================================
+# 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
+
+
+
+
+
diff --git a/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg b/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5d4e2e744baf2c25a947db4ca667f51c39fcd31d
--- /dev/null
+++ b/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
@@ -0,0 +1,183 @@
+# ==================================================================
+# 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
+
+
+
+
+
diff --git a/scenarios/LaserScannerTest/config/global.cfg b/scenarios/LaserScannerTest/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..e7cd7f67055d284001fc7ac3ad93ed5b65719366
--- /dev/null
+++ b/scenarios/LaserScannerTest/config/global.cfg
@@ -0,0 +1,7 @@
+# ==================================================================
+# Global Config from Scenario LaserScannerTest
+# ==================================================================
+
+
+
+
diff --git a/scenarios/OptoForceUnit/OptoForceUnit.scx b/scenarios/OptoForceUnit/OptoForceUnit.scx
index 217d563cb8db1edbd5df43ab05e530c71b970069..7ede5163dd4f93b710a65fd62f967a26a873b55d 100644
--- a/scenarios/OptoForceUnit/OptoForceUnit.scx
+++ b/scenarios/OptoForceUnit/OptoForceUnit.scx
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="OptoForceUnit" lastChange="2017-03-06.18:50:02" creation="2017-03-03.15:11:40" globalConfigName="./config/global.cfg" package="RobotAPI">
+<scenario name="OptoForceUnit" lastChange="2017-03-27.17:45:06" creation="2017-03-03.15:11:40" globalConfigName="./config/global.cfg" package="RobotAPI">
<application name="OptoForceUnitApp" instance="" package="RobotAPI"/>
<application name="OptoForceUnitObserverApp" instance="" package="RobotAPI"/>
</scenario>
diff --git a/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg b/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
index d36d393fe461df989545cbfd931333583a68c0aa..d51959056fafd226f9d542f64262a96e5e9384c5 100644
--- a/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
+++ b/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
@@ -74,6 +74,14 @@
# ArmarX.EnableProfiling = 0
+# ArmarX.OptoForceUnit.CalibrationFilePath: Path of the Calibration File
+# Attributes:
+# - Default: RobotAPI/sensors/OptoForceCalibration.xml
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnit.CalibrationFilePath = RobotAPI/sensors/OptoForceCalibration.xml
+
+
# ArmarX.OptoForceUnit.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: 0
@@ -98,6 +106,14 @@
# ArmarX.OptoForceUnit.ObjectName = ""
+# ArmarX.OptoForceUnit.OptoForceTopicName: Name of the OptoForce Topic
+# Attributes:
+# - Default: OptoForceValues
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OptoForceUnit.OptoForceTopicName = OptoForceValues
+
+
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: 1
diff --git a/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg b/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
index f834041ce30668ad17b90bc1f64f5efdbf7954d1..71109dc9a07bf3dc11d6e36a9b603ad2abba1b4d 100644
--- a/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
+++ b/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
@@ -186,11 +186,6 @@
# ArmarX.Verbosity = Info
-# Ice.Config: Custom Property
-# Attributes:
-# - Default: ::NOT_DEFINED::
-# - Case sensitivity: no
-# - Required: no
-# Ice.Config = ::NOT_DEFINED::
+
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index b2d194d9365882b73a00a2d5405a31739381ded4..afbd2702c93994f6eb71deec1c2e9ef711e4d768 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/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)
diff --git a/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt b/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5cbb7a44126940f6ca1c0c476189689794010e0a
--- /dev/null
+++ b/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt
@@ -0,0 +1,22 @@
+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}")
diff --git a/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp b/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..111a542188d5d46aa6f5664aa5476d9c7dc6f88e
--- /dev/null
+++ b/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * 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");
+}
diff --git a/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..510e19879be5a995f91d9b5f4795e53ec547adf1
--- /dev/null
+++ b/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt
@@ -0,0 +1,20 @@
+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}")
diff --git a/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp b/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ecae3d6b3b6a05d96acf8d1e3b661d2ed47e5f58
--- /dev/null
+++ b/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * 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");
+}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 9dabab63daeba10e73ac9734297a9eacbf4dc16a..ebf8cfff1911bad7dacae2174ae3e67b4bd36360 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/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);
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index e3453b178590a5e056112fdd171ca0b41d93cfb5..ad87ce0aa80fdb948e57bae2a6cb4d8fd7579f91 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/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
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e5980094db9c2eeac1409e2ba1544e8c4538331
--- /dev/null
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -0,0 +1,113 @@
+/*
+ * 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);
+}
+
+
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.h b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..0696ec089541842bca8c24e34030c696f2b885b9
--- /dev/null
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
@@ -0,0 +1,87 @@
+/*
+ * 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
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index 8270167be4736817298509e27d230e444cf6bc74..7d5899a24dbbd16720e616420c57146ec963a7ab 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -2,4 +2,4 @@ add_subdirectory(WeissHapticSensor)
add_subdirectory(XsensIMU)
add_subdirectory(OptoForceUnit)
add_subdirectory(OrientedTactileSensor)
-
+add_subdirectory(HokuyoLaserUnit)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d78aae9576595b3e4461178cf8c9c171ae987536
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
@@ -0,0 +1,31 @@
+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)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a03e962ea2142d4b062110f6b7155b1efd9e48c3
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -0,0 +1,204 @@
+/*
+ * 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);
+ }
+ }
+}
+
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..d40262623b13262a7473d05db05332801039b29b
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -0,0 +1,132 @@
+/*
+ * 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
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/test/CMakeLists.txt b/source/RobotAPI/drivers/HokuyoLaserUnit/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bac2679024ebfd9630fbf1950557b344cbe82708
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore HokuyoLaserUnit)
+
+armarx_add_test(HokuyoLaserUnitTest HokuyoLaserUnitTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..581618323d926db09e1559e695df1345ed865190
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::HokuyoLaserUnit
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::HokuyoLaserUnit instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
index 9fdfeaf16f6b73dea6b9d87a6a5b7959d70ca704..f83e84915ebe787f3c414fa03786f97835a93cf9 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -29,10 +29,16 @@
using namespace armarx;
+OptoForceUnit::OptoForceUnit()
+ : isRecording(false), stopRecordingFlag(false)
+{
+
+}
+
void OptoForceUnit::onInitComponent()
{
std::string calibrationFilePath = getProperty<std::string>("CalibrationFilePath").getValue();
- if(!ArmarXDataPath::getAbsolutePath(calibrationFilePath, calibrationFilePath))
+ if (!ArmarXDataPath::getAbsolutePath(calibrationFilePath, calibrationFilePath))
{
throw LocalException("Could not find calibration file '") << calibrationFilePath << "'";
}
@@ -42,9 +48,9 @@ void OptoForceUnit::onInitComponent()
auto findDaqNode = [&](std::string serialNumber)
{
- for(RapidXmlReaderNode daqNode : rootNode.nodes("Daq"))
+ for (RapidXmlReaderNode daqNode : rootNode.nodes("Daq"))
{
- if(daqNode.attribute_value("serialNumber") == serialNumber)
+ if (daqNode.attribute_value("serialNumber") == serialNumber)
{
return daqNode;
}
@@ -67,7 +73,7 @@ void OptoForceUnit::onInitComponent()
std::string serialNumber(portlist[i].serialNumber);
ARMARX_INFO << "Found DAQ: deviceName='" << deviceName << "', serialNumber='" << serialNumber << "'";
RapidXmlReaderNode daqNode = findDaqNode(serialNumber);
- if(!daqNode.is_valid())
+ if (!daqNode.is_valid())
{
throw LocalException("Could not find config node for device deviceName='") << deviceName << "', serialNumber='" << serialNumber << "'";
}
@@ -105,11 +111,11 @@ void OptoForceUnit::run()
OptoForceUnitListenerPrx batchPrx = topicPrx->ice_batchOneway();
ARMARX_IMPORTANT << "got batch Proxy";
- while(readTask->isRunning())
+ while (readTask->isRunning())
{
bool flushNeeded = false;
- for(const DaqWrapperPtr& daqPtr : daqList)
+ for (const DaqWrapperPtr& daqPtr : daqList)
{
OptoPackage* pa = 0;
int size = daqPtr->daq.readAll(pa, false);
@@ -118,7 +124,7 @@ void OptoForceUnit::run()
// size == 0: no new data for daq
continue;
}
- if(size < 0)
+ if (size < 0)
{
// size == -1: buffer full
ARMARX_WARNING << "buffer full of daq " << daqPtr->deviceName;
@@ -128,24 +134,62 @@ void OptoForceUnit::run()
IceUtil::Time now = IceUtil::Time::now();
TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
- for (int i = 0; i < daqPtr->daq.getSensorSize(); i++)
+ int sensorCount = daqPtr->daq.getSensorSize();
+
+ std::vector<std::array<float, 3>> data;
+ data.resize(sensorCount);
+
+ for (int i = 0; i < sensorCount; i++)
{
+ if (!daqPtr->enableFlags.at(i))
+ {
+ continue;
+ }
for (int n = 0; n < size; n++)
{
float countsPerN = daqPtr->countsPerN.at(i);
- float x = pa[i * size + n].x / countsPerN;
- float y = pa[i * size + n].y / countsPerN;
- float z = pa[i * size + n].z / countsPerN;
+ float x = pa[i * size + n].x / countsPerN - daqPtr->offsets.at(i).at(0);
+ float y = pa[i * size + n].y / countsPerN - daqPtr->offsets.at(i).at(1);
+ float z = pa[i * size + n].z / countsPerN - daqPtr->offsets.at(i).at(2);
batchPrx->reportSensorValues(daqPtr->deviceName + ":" + std::to_string(i), daqPtr->sensorNames.at(i), x, y, z, nowTimestamp);
flushNeeded = true;
+ data.at(i) = {x, y, z};
}
}
-
delete[] pa;
+
+ if (isRecording)
+ {
+ recordingFile << "{\"timestamp\":\"" << now.toDateTime() << "\",";
+ recordingFile << "\"daq\":\"" << daqPtr->serialNumber << "\",\"data\":[";
+ bool first = true;
+ for (int i = 0; i < sensorCount; i++)
+ {
+ if (!daqPtr->enableFlags.at(i))
+ {
+ continue;
+ }
+ if (!first)
+ {
+ recordingFile << ",";
+ }
+ first = false;
+ recordingFile << "[" << data.at(i).at(0) << "," << data.at(i).at(1) << "," << data.at(i).at(2) << "]";
+ }
+ recordingFile << "]}\n";
+ recordingFile.flush();
+ }
+
}
}
- if(flushNeeded)
+ if (isRecording && stopRecordingFlag)
+ {
+ recordingFile.close();
+ isRecording = false;
+ }
+ stopRecordingFlag = false;
+ if (flushNeeded)
{
batchPrx->ice_flushBatchRequests();
}
@@ -171,7 +215,7 @@ void OptoForceUnit::onExitComponent()
armarx::PropertyDefinitionsPtr OptoForceUnit::createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr(new OptoForceUnitPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
}
@@ -180,13 +224,15 @@ OptoForceUnit::DaqWrapper::DaqWrapper(const std::string& deviceName, const std::
: deviceName(deviceName), serialNumber(serialNumber)
{
int i = 0;
- for(RapidXmlReaderNode sensorNode : daqNode.nodes("Sensor"))
+ for (RapidXmlReaderNode sensorNode : daqNode.nodes("Sensor"))
{
float counts_at_nc = sensorNode.attribute_as_float("counts_at_nc");
float nominalCapacity = sensorNode.attribute_as_float("nominalCapacity");
std::string sensorName = sensorNode.attribute_value_or_default("name", serialNumber + "-" + std::to_string(i));
countsPerN.push_back(counts_at_nc / nominalCapacity);
sensorNames.push_back(sensorName);
+ enableFlags.push_back(sensorNode.attribute_as_optional_bool("enabled", "true", "false", true));
+ offsets.push_back({sensorNode.attribute_as_float("offsetX"), sensorNode.attribute_as_float("offsetY"), sensorNode.attribute_as_float("offsetZ")});
i++;
}
}
@@ -212,9 +258,22 @@ void OptoForceUnit::DaqWrapper::printInfo()
void OptoForceUnit::DaqWrapper::checkSensorCount()
{
- if((int)countsPerN.size() != daq.getSensorSize())
+ if ((int)countsPerN.size() != daq.getSensorSize())
{
throw LocalException("Sensor count mismatch. Configured: ") << ((int)countsPerN.size()) << " found: " << daq.getSensorSize();
}
ARMARX_INFO_S << "Configured: " << ((int)countsPerN.size()) << " found: " << daq.getSensorSize() << " sensors";
}
+
+
+void armarx::OptoForceUnit::startRecording(const std::string& filepath, const Ice::Current&)
+{
+ ARMARX_IMPORTANT << "start recording: " << filepath;
+ recordingFile.open(filepath);
+ isRecording = true;
+}
+
+void armarx::OptoForceUnit::stopRecording(const Ice::Current&)
+{
+ stopRecordingFlag = true;
+}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
index 50c7fccdd3404fba75dd010cde72e7f37c92cc7a..fe9178f7abde3ea77ed545641b3affa2603a84bd 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/OptoForceUnit.h>
-#include <OptoForce/omd/opto.h>
+#include <opto.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
namespace armarx
@@ -53,14 +53,15 @@ namespace armarx
* @defgroup Component-OptoForceUnit OptoForceUnit
* @ingroup RobotAPI-Components
* A description of the component OptoForceUnit.
- *
+ *
* @class OptoForceUnit
* @ingroup Component-OptoForceUnit
* @brief Brief description of class OptoForceUnit.
- *
+ *
* Detailed description of class OptoForceUnit.
*/
class OptoForceUnit :
+ virtual public OptoForceUnitInterface,
virtual public armarx::Component
{
private:
@@ -74,6 +75,8 @@ namespace armarx
std::string serialNumber;
std::vector<float> countsPerN;
std::vector<std::string> sensorNames;
+ std::vector<bool> enableFlags;
+ std::vector<std::array<float, 3>> offsets;
void printInfo();
void checkSensorCount();
@@ -81,6 +84,8 @@ namespace armarx
typedef boost::shared_ptr<DaqWrapper> DaqWrapperPtr;
public:
+ OptoForceUnit();
+
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
@@ -125,6 +130,16 @@ namespace armarx
std::vector<DaqWrapperPtr> daqList;
RunningTask<OptoForceUnit>::pointer_type readTask;
+ std::ofstream recordingFile;
+ bool isRecording;
+ bool stopRecordingFlag;
+
+
+
+ // OptoForceUnitInterface interface
+ public:
+ void startRecording(const std::string& filepath, const Ice::Current&);
+ void stopRecording(const Ice::Current&);
};
}
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 8e1b05f0d2b9484747993691c5ece3f36addc723..fc363790ab3bf0da2fcbdd97a12daa1c84050093 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -10,3 +10,5 @@ add_subdirectory(RobotViewerPlugin)
add_subdirectory(DebugDrawerViewer)
add_subdirectory(ViewSelection)
+
+add_subdirectory(LaserScannerPlugin)
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/LaserScannerPlugin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..29613a7c613f09b80c711ddd8663f4cbd14d4f53
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/CMakeLists.txt
@@ -0,0 +1,42 @@
+
+armarx_set_target("LaserScannerPluginGuiPlugin")
+
+find_package(Qt4 COMPONENTS QtCore QtGui QtDesigner)
+
+armarx_build_if(QT_FOUND "Qt not available")
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+if(QT_FOUND)
+ include(${QT_USE_FILE})
+endif()
+
+set(SOURCES
+./LaserScannerPluginGuiPlugin.cpp ./LaserScannerPluginWidgetController.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.cpp @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.cpp
+)
+
+set(HEADERS
+./LaserScannerPluginGuiPlugin.h ./LaserScannerPluginWidgetController.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.h @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS
+./LaserScannerPluginWidget.ui
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@Widget.ui
+)
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS
+ RobotAPICore
+ SimpleConfigDialog ${QT_LIBRARIES})
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library(LaserScannerPluginGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c391a0ff66ebe5698e8f4f4b62b2a874374ea7fd
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
@@ -0,0 +1,34 @@
+/*
+ * 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::gui-plugins::LaserScannerPluginGuiPlugin
+ * \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 "LaserScannerPluginGuiPlugin.h"
+
+#include "LaserScannerPluginWidgetController.h"
+
+using namespace armarx;
+
+LaserScannerPluginGuiPlugin::LaserScannerPluginGuiPlugin()
+{
+ addWidget < LaserScannerPluginWidgetController > ();
+}
+
+Q_EXPORT_PLUGIN2(armarx_gui_LaserScannerPluginGuiPlugin, LaserScannerPluginGuiPlugin)
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..546adaed422fde8fc1812a1928c528c0e5475700
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
@@ -0,0 +1,51 @@
+/*
+ * 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::gui-plugins::LaserScannerPlugin
+ * \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_LaserScannerPlugin_GuiPlugin_H
+#define _ARMARX_RobotAPI_LaserScannerPlugin_GuiPlugin_H
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+ /**
+ * \class LaserScannerPluginGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief LaserScannerPluginGuiPlugin brief description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerPluginGuiPlugin:
+ public armarx::ArmarXGuiPlugin
+ {
+ public:
+ /**
+ * All widgets exposed by this plugin are added in the constructor
+ * via calls to addWidget()
+ */
+ LaserScannerPluginGuiPlugin();
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..eac4cfc57df775ec9a416acbe1e6d2a58794980c
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
@@ -0,0 +1,128 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>LaserScannerPluginWidget</class>
+ <widget class="QWidget" name="LaserScannerPluginWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>490</width>
+ <height>379</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>LaserScannerPluginWidget</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetMinAndMaxSize</enum>
+ </property>
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Ignored" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Device:</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QComboBox" name="deviceComboBox"/>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Frame:</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Maximum Angle (rad)</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Minimum Angle (rad):</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Step Size (rad):</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLabel" name="frameLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="minAngleLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="maxAngleLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="stepSizeLabel">
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QGraphicsView" name="graphicsView">
+ <property name="verticalScrollBarPolicy">
+ <enum>Qt::ScrollBarAlwaysOff</enum>
+ </property>
+ <property name="horizontalScrollBarPolicy">
+ <enum>Qt::ScrollBarAlwaysOff</enum>
+ </property>
+ <property name="resizeAnchor">
+ <enum>QGraphicsView::AnchorViewCenter</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01a0f8d8cb851b70b8ae3025099f059926cdeca5
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
@@ -0,0 +1,200 @@
+/*
+ * 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::gui-plugins::LaserScannerPluginWidgetController
+ * \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 "LaserScannerPluginWidgetController.h"
+
+#include <QGraphicsView>
+#include <QGraphicsLineItem>
+
+#include <string>
+
+using namespace armarx;
+
+LaserScannerPluginWidgetController::LaserScannerPluginWidgetController()
+{
+ widget.setupUi(getWidget());
+}
+
+
+LaserScannerPluginWidgetController::~LaserScannerPluginWidgetController()
+{
+
+}
+
+
+void LaserScannerPluginWidgetController::loadSettings(QSettings* settings)
+{
+
+}
+
+void LaserScannerPluginWidgetController::saveSettings(QSettings* settings)
+{
+
+}
+
+
+void LaserScannerPluginWidgetController::onInitComponent()
+{
+ usingProxy(laserScannerUnitName);
+
+ connect(this, SIGNAL(newSensorValuesReported()), this, SLOT(onNewSensorValuesReported()), Qt::QueuedConnection);
+ connect(widget.deviceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelected(int)));
+}
+
+
+void LaserScannerPluginWidgetController::onConnectComponent()
+{
+ laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
+ std::string topicName = laserScannerUnit->getReportTopicName();
+ usingTopic(topicName);
+
+ laserScanners = laserScannerUnit->getConnectedDevices();
+}
+
+QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
+{
+ if (!dialog)
+ {
+ dialog = new SimpleConfigDialog(parent);
+ dialog->addProxyFinder<LaserScannerUnitInterfacePrx>({"LaserScannerUnit", "", "*"});
+ }
+ return qobject_cast<SimpleConfigDialog*>(dialog);
+}
+
+void LaserScannerPluginWidgetController::configured()
+{
+ if (dialog)
+ {
+ laserScannerUnitName = dialog->getProxyName("LaserScannerUnit");
+ }
+}
+
+void LaserScannerPluginWidgetController::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& newScan, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+ {
+ boost::mutex::scoped_lock lock(scanMutex);
+
+ LaserScan& scan = scans[device];
+ // TODO: Do some filtering? aggregation?
+ scan = newScan;
+ }
+
+ emit newSensorValuesReported();
+}
+
+void LaserScannerPluginWidgetController::onNewSensorValuesReported()
+{
+ QComboBox* deviceBox = widget.deviceComboBox;
+
+ boost::mutex::scoped_lock lock(scanMutex);
+ for (auto & pair : scans)
+ {
+ QString deviceName(QString::fromStdString(pair.first.c_str()));
+ if (deviceBox->findText(deviceName) < 0)
+ {
+ deviceBox->addItem(deviceName);
+ }
+ }
+
+ std::string deviceName(deviceBox->currentText().toUtf8().data());
+ float stepSize = 0.25f;
+ for (LaserScannerInfo const & scanner : laserScanners)
+ {
+ if (scanner.device == deviceName)
+ {
+ stepSize = scanner.stepSize;
+ }
+ }
+
+ QGraphicsView* view = widget.graphicsView;
+
+ scene.reset(new QGraphicsScene());
+ widget.graphicsView->setScene(scene.get());
+ int outerR = std::min(view->width() / 2, view->height() / 2);
+ scene->addEllipse(-outerR, -outerR, 2 * outerR, 2 * outerR, QPen(QColor(255, 255, 255)));
+ int r = outerR - 10;
+ //scene.addEllipse(-r, -r, 2 * r, 2 * r, QPen(QColor(200, 200, 200)));
+ QColor stepColor(QColor::fromRgb(100, 100, 255));
+ QPen stepPen(stepColor);
+ QBrush stepBrush(stepColor);
+ auto line = [&](float angle, float d)
+ {
+ float di = d * r;
+ QGraphicsEllipseItem* item = scene->addEllipse(-di, -di, 2 * di, 2 * di, stepPen, stepBrush);
+ // Angles for Qt ellipse are in 16th of degree (who thought that would be a great idea?)
+ item->setStartAngle(std::round(16.0f * angle * 180.0 / M_PI) + 90 * 16);
+ item->setSpanAngle(std::round(stepSize * 16.0f * 180.0 / M_PI));
+ };
+
+ LaserScan& scan = scans[deviceName];
+ float maxDistance = 1000.0f;
+ for (LaserScanStep & step : scan)
+ {
+ if (step.distance > maxDistance)
+ {
+ maxDistance = step.distance;
+ }
+ }
+ float ringDistance = 1000.0f;
+ int numberOfRings = (std::size_t)std::ceil(maxDistance / ringDistance);
+ std::deque<int>& history = numberOfRingsHistory[deviceName];
+ history.push_back(numberOfRings);
+ if (history.size() > 256)
+ {
+ history.pop_front();
+ }
+ int maxNumberOfRings = *std::max_element(history.begin(), history.end());
+ float outerRadius = maxNumberOfRings * ringDistance;
+
+ for (LaserScanStep & step : scan)
+ {
+ line(step.angle, step.distance / outerRadius);
+ }
+
+ for (int ringIndex = 1; ringIndex <= maxNumberOfRings; ++ringIndex)
+ {
+ float ri = 1.0f * ringIndex / maxNumberOfRings * r;
+ scene->addEllipse(-ri, -ri, 2 * ri, 2 * ri, QPen(QColor(200, 200, 200)));
+ }
+
+ view->fitInView(scene->itemsBoundingRect(), Qt::KeepAspectRatio);
+}
+
+void LaserScannerPluginWidgetController::onDeviceSelected(int index)
+{
+ if (index < 0 && index >= widget.deviceComboBox->count())
+ {
+ return;
+ }
+ std::string deviceName = widget.deviceComboBox->itemText(index).toStdString();
+ for (LaserScannerInfo const & scanner : laserScanners)
+ {
+ if (scanner.device == deviceName)
+ {
+ widget.frameLabel->setText(QString::fromStdString(scanner.frame));
+ widget.minAngleLabel->setText(QString::number(scanner.minAngle));
+ widget.maxAngleLabel->setText(QString::number(scanner.minAngle));
+ widget.stepSizeLabel->setText(QString::number(scanner.stepSize));
+ }
+ }
+
+}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..61de784ae16b9626c5c8962378d9383a4da7eedd
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
@@ -0,0 +1,132 @@
+/*
+ * 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::gui-plugins::LaserScannerPluginWidgetController
+ * @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_LaserScannerPlugin_WidgetController_H
+#define _ARMARX_RobotAPI_LaserScannerPlugin_WidgetController_H
+
+#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx
+{
+ /**
+ \page ArmarXGui-GuiPlugins-LaserScannerPlugin LaserScannerPlugin
+ \brief The LaserScannerPlugin allows visualizing the raw sensor data captured from multiple laser scanners.
+
+ API Documentation \ref LaserScannerPluginWidgetController
+
+ \see LaserScannerPluginGuiPlugin
+ */
+
+ /**
+ * \class LaserScannerPluginWidgetController
+ * \brief LaserScannerPluginWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ LaserScannerPluginWidgetController:
+ public armarx::ArmarXComponentWidgetController,
+ public armarx::LaserScannerUnitListener
+ {
+ Q_OBJECT
+
+ public:
+ /**
+ * Controller Constructor
+ */
+ explicit LaserScannerPluginWidgetController();
+
+ /**
+ * Controller destructor
+ */
+ virtual ~LaserScannerPluginWidgetController();
+
+ /**
+ * @see ArmarXWidgetController::loadSettings()
+ */
+ virtual void loadSettings(QSettings* settings);
+
+ /**
+ * @see ArmarXWidgetController::saveSettings()
+ */
+ virtual void saveSettings(QSettings* settings);
+
+ /**
+ * Returns the Widget name displayed in the ArmarXGui to create an
+ * instance of this class.
+ */
+ virtual QString getWidgetName() const
+ {
+ return "RobotControl.LaserScannerGUI";
+ }
+
+ /**
+ * \see armarx::Component::onInitComponent()
+ */
+ virtual void onInitComponent();
+
+ /**
+ * \see armarx::Component::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+
+ QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+
+ void configured() override;
+
+ void reportSensorValues(const std::string& device, const std::string& name,
+ const LaserScan& scan, const TimestampBasePtr& timestamp,
+ const Ice::Current& c) override;
+ public slots:
+ void onNewSensorValuesReported();
+ void onDeviceSelected(int index);
+
+ signals:
+ void newSensorValuesReported();
+
+ private:
+ /**
+ * Widget Form
+ */
+ Ui::LaserScannerPluginWidget widget;
+ QPointer<SimpleConfigDialog> dialog;
+
+ std::string laserScannerUnitName;
+ LaserScannerUnitInterfacePrx laserScannerUnit;
+ LaserScannerInfoSeq laserScanners;
+
+ Mutex scanMutex;
+ std::unordered_map<std::string, LaserScan> scans;
+ std::unordered_map<std::string, std::deque<int>> numberOfRingsHistory;
+
+
+ std::unique_ptr<QGraphicsScene> scene;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index cc2ef98360a49b7d25c6761d3709a2fd87608b7d..35e0c8122b8e61dd55562a2df8d9b1f500cd3729 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -24,6 +24,7 @@ set(SLICE_FILES
units/ForceTorqueUnit.ice
units/InertialMeasurementUnit.ice
units/OptoForceUnit.ice
+ units/LaserScannerUnit.ice
units/HandUnitInterface.ice
units/HapticUnit.ice
units/WeissHapticUnit.ice
diff --git a/source/RobotAPI/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..f022260d03021a6850c99f3dfdc61b61e33555b0
--- /dev/null
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -0,0 +1,86 @@
+/*
+ * 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
+ * @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_UNITS_LASER_SCANNER_UNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_LASER_SCANNER_UNIT_SLICE_
+
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Matrix.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
+
+
+module armarx
+{
+ /**
+ * 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 LaserScanStep
+ {
+ float angle;
+ float distance;
+ };
+
+ struct LaserScannerInfo
+ {
+ string device;
+ string frame;
+ float minAngle;
+ float maxAngle;
+ float stepSize;
+ };
+
+ sequence<LaserScanStep> LaserScan;
+ sequence<LaserScannerInfo> LaserScannerInfoSeq;
+
+ interface LaserScannerUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ ["cpp:const"]
+ idempotent string getReportTopicName() throws NotInitializedException;
+
+ ["cpp:const"]
+ LaserScannerInfoSeq getConnectedDevices();
+ };
+
+ interface LaserScannerUnitListener
+ {
+ void reportSensorValues(string device, string name, LaserScan values, TimestampBase timestamp);
+ };
+
+ interface LaserScannerUnitObserverInterface extends ObserverInterface, LaserScannerUnitListener
+ {
+ };
+
+};
+
+#endif
diff --git a/source/RobotAPI/interface/units/OptoForceUnit.ice b/source/RobotAPI/interface/units/OptoForceUnit.ice
index c0085c06fa1a9838ad6e790440ffb3ff0a457a7c..553aa042877688b333a0ee2a4e6de86431ae5196 100644
--- a/source/RobotAPI/interface/units/OptoForceUnit.ice
+++ b/source/RobotAPI/interface/units/OptoForceUnit.ice
@@ -43,8 +43,10 @@ module armarx
/**
* Implements an interface to an OptoForceUnit.
**/
- interface OptoForceUnitInterface extends armarx::SensorActorUnitInterface
+ interface OptoForceUnitInterface //extends armarx::SensorActorUnitInterface
{
+ void startRecording(string filepath);
+ void stopRecording();
};
/**
* Implements an interface to an OptoForceUnitListener.
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 1e629a61b88b7941bc7a828f104009981eb1caca..0a25c6ee696e7a3a8fbb72c39aae65345078812a 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -189,6 +189,8 @@ module armarx
DebugDrawerLineSetPointList points;
DebugDrawerLineSetIntensityList intensities;
+
+ bool useHeatMap = false;
};
sequence< Vector3Base > PolygonPointList;
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
index acb5582b8f8ba728bd45f9e099ade3cd07bd5a14..3bdf59b6936735dd4d946bd5f13e0b8b15219737 100644
--- a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
@@ -34,6 +34,7 @@
#include <RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h>
#include <RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h>
#include <RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h>
namespace armarx
{
@@ -134,9 +135,38 @@ namespace armarx
}
};
+ template <>
+ struct PassThroughControllerTargetTypeTraits<PlatformWheelVelocityTarget>
+ {
+ static float* getTargetDatamember(PlatformWheelVelocityTarget& t)
+ {
+ return &t.velocity;
+ }
+ static const float* getFrontRightWheelVelocity(const PlatformDataUnitInterface* pu)
+ {
+ return pu->getFrontRightWheelVelocity();
+ }
+ static const float* getFrontLeftWheelVelocity(const PlatformDataUnitInterface* pu)
+ {
+ return pu->getFrontLeftWheelVelocity();
+ }
+ static const float* getRearRightWheelVelocity(const PlatformDataUnitInterface* pu)
+ {
+ return pu->getRearRightWheelVelocity();
+ }
+ static const float* getRearLeftWheelVelocity(const PlatformDataUnitInterface* pu)
+ {
+ return pu->getRearLeftWheelVelocity();
+ }
+ static std::string getControlMode()
+ {
+ return ControlModes::VelocityMode;
+ }
+ };
+
LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
- LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController("JointPassThroughController");
+ LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController("TorquePassThroughController");
template <typename TargetType>
std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
@@ -194,6 +224,8 @@ namespace armarx
for (std::size_t i = 0; i < jointStates.size(); ++i)
{
iceTargets.at(i).val.store(*jointStates.at(i));
+ //set it to zero
+ iceTargets.at(i).val.store(0.0);
}
}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
index 7c525d1525de28f8bdf93001ae9d6ae3183c660c..860a626f9ff9f45b9817f908e3a70e52a2212af1 100644
--- a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -302,6 +302,12 @@ namespace armarx
controlDataTripleBuffer {initialCommands}
{
}
+
+ virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+ {
+ rtUpdateControlStruct();
+ rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
protected:
const ControlDataStruct& rtGetControlStruct() const
{
@@ -311,11 +317,6 @@ namespace armarx
{
return controlDataTripleBuffer.updateReadBuffer();
}
- virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
- {
- rtUpdateControlStruct();
- rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
void writeControlStruct()
{
@@ -336,5 +337,27 @@ namespace armarx
};
template <typename ControlDataStruct>
using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
+
+
+ struct PlatformCartesianVelocity
+ {
+ float vx;
+ float vy;
+ float vAngle;
+ PlatformCartesianVelocity() : vx(0), vy(0), vAngle(0)
+ { }
+ };
+
+
+ class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity>
+ {
+
+ public:
+ virtual void setTarget(float vx, float vy, float vAngle) = 0;
+ };
+
+ typedef boost::shared_ptr <AbstractLvl1PlatformVelocityController> AbstractLvl1PlatformVelocityControllerPtr;
+
+
}
#endif
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 66dfd49060d6573f80024f3af7f22bc299169315..f6fdfe61ccb88fcf5e7eccfac7798cd5f64e868a 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -21,6 +21,8 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -32,6 +34,12 @@
using namespace Eigen;
using namespace std;
+
+template class ::IceInternal::Handle<::armarx::FramedPose>;
+template class ::IceInternal::Handle<::armarx::FramedPosition>;
+template class ::IceInternal::Handle<::armarx::FramedDirection>;
+template class ::IceInternal::Handle<::armarx::FramedOrientation>;
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 257d8f95a2f7046dbc9a3ebd06be2ee5868e91e5..cce0aedfd3d136e8db173197a0b652cdb77fdeac 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -363,4 +363,12 @@ namespace armarx
typedef IceInternal::Handle<FramedPose> FramedPosePtr;
}
+
+extern template class ::IceInternal::Handle<::armarx::FramedPose>;
+extern template class ::IceInternal::Handle<::armarx::FramedPosition>;
+extern template class ::IceInternal::Handle<::armarx::FramedDirection>;
+extern template class ::IceInternal::Handle<::armarx::FramedOrientation>;
+
+
+
#endif
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index a56c01fb0826e3f9f369e2237c174b8945bb7703..f6e8963d3ac67555c690e60df458bcf66595b4cf 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -15,12 +15,13 @@
* 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
+ * @package
+ * @author
+ * @date
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+#define EIGEN_NO_STATIC_ASSERT
#include "Pose.h"
#include <VirtualRobot/Robot.h>
@@ -31,6 +32,19 @@
using namespace Eigen;
using namespace std;
+template class ::IceInternal::Handle<::armarx::Pose>;
+template class ::IceInternal::Handle<::armarx::Vector2>;
+template class ::IceInternal::Handle<::armarx::Vector3>;
+template class ::IceInternal::Handle<::armarx::Quaternion>;
+
+namespace Eigen
+{
+ template class Matrix<float, 3, 1>;
+ template class Matrix<float, 3, 3>;
+ template class Matrix<float, 4, 4>;
+}
+
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index d9cadc18bd23ecbf8197f721f16c62444de60323..424da519184ab454c9f894316e0c22497f9ae464 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -270,4 +270,16 @@ namespace armarx
typedef IceInternal::Handle<Pose> PosePtr;
}
+
+extern template class ::IceInternal::Handle<::armarx::Pose>;
+extern template class ::IceInternal::Handle<::armarx::Vector2>;
+extern template class ::IceInternal::Handle<::armarx::Vector3>;
+extern template class ::IceInternal::Handle<::armarx::Quaternion>;
+
+namespace Eigen
+{
+ extern template class Matrix<float, 3, 1>;
+ extern template class Matrix<float, 3, 3>;
+ extern template class Matrix<float, 4, 4>;
+}
#endif
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.h b/source/RobotAPI/libraries/core/RobotStatechartContext.h
index 369ca866585f90f75be4b603df7d148b197cf6d2..79c1a76d4fc4324bd4f00cab00b30432e5ca44bf 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.h
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.h
@@ -69,7 +69,7 @@ namespace armarx
{
public:
// inherited from Component
- virtual std::string getDefaultName()
+ virtual std::string getDefaultName() const
{
return "RobotStatechartContext";
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 082e6fa7a3c2acbea62c3863dac7c3951226e5a1..13891cfb6f89acd3c26192c4ad13f3374cbdec2d 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -111,8 +111,7 @@ namespace armarx
obj->getDoubleArray("timestamps", timestamps);
for (size_t row = 0; row < rowContent.size(); row++)
{
- Ice::StringSeq values;
- boost::split(values, rowContent[row], boost::is_any_of(","));
+ Ice::StringSeq values = Split(rowContent[row], ",");
Ice::DoubleSeq newRow;
newRow.reserve(values.size());
for (std::string v : values)
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 476776366726ff12619939e4690d7d4bb8eaf9ce..16d718d9b110fb7fa214a69f25959d734fc2b4d9 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -30,6 +30,10 @@
#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
+
+
+
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <Eigen/Geometry>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -71,13 +75,13 @@ namespace armarx
}
- RobotNodePtr RemoteRobot::getRootNode()
+ RobotNodePtr RemoteRobot::getRootNode() const
{
+ // lazy initialization needed since shared_from_this() must not be called in constructor
if (!_root)
{
_root = RemoteRobot::createRemoteRobotNode(_robot->getRootNode(), shared_from_this());
}
-
return _root;
}
@@ -155,7 +159,7 @@ namespace armarx
return _robot->hasRobotNodeSet(name);
}
- RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const string& nodeSetName)
+ RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const string& nodeSetName) const
{
vector<RobotNodePtr> storeNodes;
RobotNodeSetInfoPtr info = _robot->getRobotNodeSet(nodeSetName);
@@ -164,7 +168,7 @@ namespace armarx
}
- void RemoteRobot::getRobotNodeSets(vector<RobotNodeSetPtr>& storeNodeSet)
+ void RemoteRobot::getRobotNodeSets(vector<RobotNodeSetPtr>& storeNodeSet) const
{
NameList sets = _robot->getRobotNodeSets();
@@ -398,7 +402,7 @@ namespace armarx
else
{
Ice::StringSeq includePaths;
- for (const std::string & projectName : packages)
+ for (const std::string& projectName : packages)
{
if (projectName.empty())
{
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 3fa32389a9bc6a0302aaa0f77584b004076f6e09..496899ec87deed0478a8057ed459831eb34047ed 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -21,29 +21,33 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
+#pragma once
#ifndef _ARMARX_REMOTE_ROBOT_H__
#define _ARMARX_REMOTE_ROBOT_H__
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/interface/core/RobotState.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-
+#include <VirtualRobot/XML/RobotIO.h>
// boost
#include <boost/thread/mutex.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_base_of.hpp>
+//namespace VirtualRobot
+//{
+// class RobotNodeRevolute;
+// class RobotNodePrismatic;
+// class RobotNodeFixed;
+//}
+
namespace armarx
{
// forward declaration of RemoteRobotNode
@@ -153,7 +157,7 @@ namespace armarx
RemoteRobot(SharedRobotInterfacePrx robot);
- virtual VirtualRobot::RobotNodePtr getRootNode();
+ virtual VirtualRobot::RobotNodePtr getRootNode() const;
virtual bool hasRobotNode(const std::string& robotNodeName);
virtual bool hasRobotNode(VirtualRobot::RobotNodePtr);
@@ -162,8 +166,8 @@ namespace armarx
virtual void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true);
virtual bool hasRobotNodeSet(const std::string& name);
- virtual VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName);
- virtual void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet);
+ virtual VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const;
+ virtual void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const;
/**
*
@@ -261,7 +265,7 @@ namespace armarx
protected:
SharedRobotInterfacePrx _robot;
std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
- VirtualRobot::RobotNodePtr _root;
+ mutable VirtualRobot::RobotNodePtr _root;
static boost::recursive_mutex m;
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt b/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt
index b0fea04f1a90ab5dc83cc8dd3c2938bca46b7637..942e26f9d5c12247b4f53649e95f8ec0c377eb0e 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt
@@ -38,11 +38,11 @@ WeissHapticGroupRemoteStateOfferer.cpp
set(HEADERS
WeissHapticGroupRemoteStateOfferer.h
-WeissHapticGroupStatechartContext.generated.h
+WeissHapticGroup.scgxml
./WeissHapticSensorTest.h
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
-./WeissHapticSensorTest.generated.h
-#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.generated.h
+./WeissHapticSensorTest.xml
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.xml
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
index f9f78a7ca3843f868e16dfc20bf3da8661228229..5f7a0472c1ab973f2f32f66e8e35f973f4a84550 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
@@ -6,4 +6,3 @@
</Proxies>
<State filename="WeissHapticSensorTest.xml" visibility="public"/>
</StatechartGroup>
-
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
deleted file mode 100644
index 82ecaf358a76c0e2a07b6566c5137ff94f34d6a8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
+++ /dev/null
@@ -1,88 +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
- */
-#ifndef ARMARX_COMPONENT_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICGROUPSTATECHARTCONTEXT_H
-#define ARMARX_COMPONENT_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICGROUPSTATECHARTCONTEXT_H
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXCore/statechart/StatechartContext.h>
-#include <RobotAPI/interface/units/HapticUnit.h>
-#include <RobotAPI/interface/units/WeissHapticUnit.h>
-
-namespace armarx
-{
- namespace WeissHapticGroup
- {
- class WeissHapticGroupStatechartContext
- : virtual public StatechartContext
- {
- protected:
- class PropertyDefinitions
- : public StatechartContextPropertyDefinitions
- {
- public:
- PropertyDefinitions(std::string prefix)
- : StatechartContextPropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>("HapticUnitObserverName", "HapticUnitObserver", "Name of the haptic unit observer that should be used");
- defineOptionalProperty<std::string>("WeissHapticUnitName", "WeissHapticUnit", "Name of the weiss haptic unit that should be used");
- }
- }; // class PropertyDefinitions
-
- private:
- HapticUnitObserverInterfacePrx hapticObserver;
- WeissHapticUnitInterfacePrx weissHapticUnit;
-
- public:
- std::string getDefaultName() const
- {
- return "WeissHapticGroupStatechartContext";
- }
- virtual void onInitStatechartContext()
- {
- usingProxy(getProperty<std::string>("HapticUnitObserverName").getValue());
- usingProxy(getProperty<std::string>("WeissHapticUnitName").getValue());
- }
- virtual void onConnectStatechartContext()
- {
- hapticObserver = getProxy<HapticUnitObserverInterfacePrx>(getProperty<std::string>("HapticUnitObserverName").getValue());
- weissHapticUnit = getProxy<WeissHapticUnitInterfacePrx>(getProperty<std::string>("WeissHapticUnitName").getValue());
- }
- HapticUnitObserverInterfacePrx getHapticObserver() const
- {
- return hapticObserver;
- }
- WeissHapticUnitInterfacePrx getWeissHapticUnit() const
- {
- return weissHapticUnit;
- }
- virtual PropertyDefinitionsPtr createPropertyDefinitions()
- {
- return PropertyDefinitionsPtr(new WeissHapticGroupStatechartContext::PropertyDefinitions(getConfigIdentifier()));
- }
- }; // class WeissHapticGroupStatechartContext
- } // namespace WeissHapticGroup
-} // namespace armarx
-
-#endif // ARMARX_COMPONENT_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICGROUPSTATECHARTCONTEXT_H
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
index 679050db723500959d4a6fa03e0e444348af2525..f825d076038ced8b26b373a110eb44dd39e74b4c 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
@@ -27,10 +27,12 @@
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
using namespace armarx;
using namespace WeissHapticGroup;
-// DO NOT EDIT NEXT LINE
+//// DO NOT EDIT NEXT LINE
WeissHapticSensorTest::SubClassRegistry WeissHapticSensorTest::Registry(WeissHapticSensorTest::GetName(), &WeissHapticSensorTest::CreateInstance);
@@ -40,10 +42,10 @@ WeissHapticSensorTest::WeissHapticSensorTest(const XMLStateConstructorParams& st
{
}
+
void WeissHapticSensorTest::onEnter()
{
- WeissHapticGroupStatechartContext* context = getContext<WeissHapticGroupStatechartContext>();
- HapticUnitObserverInterfacePrx hapticObserver = context->getHapticObserver();
+ HapticUnitObserverInterfacePrx hapticObserver = getHapticObserver();
ChannelRegistry channels = hapticObserver->getAvailableChannels(false);
std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap;
@@ -110,7 +112,6 @@ void WeissHapticSensorTest::onExit()
}
-
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr WeissHapticSensorTest::CreateInstance(XMLStateConstructorParams stateData)
{
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
deleted file mode 100644
index bea113238883938df52a52d4d1b7e05bfc6eb5f5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
+++ /dev/null
@@ -1,143 +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
- */
-#ifndef _ARMARX_XMLUSERCODE_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICSENSORTESTGENERATEDBASE_H
-#define _ARMARX_XMLUSERCODE_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICSENSORTESTGENERATEDBASE_H
-
-#include <ArmarXCore/statechart/xmlstates/XMLState.h>
-#include "WeissHapticGroupStatechartContext.generated.h"
-#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <RobotAPI/interface/units/HapticUnit.h>
-#include <RobotAPI/interface/units/WeissHapticUnit.h>
-
-namespace armarx
-{
- namespace WeissHapticGroup
- {
- template<typename StateType>
- class WeissHapticSensorTestGeneratedBase
- : virtual public XMLStateTemplate < StateType >,
- public XMLStateFactoryBase
- {
- protected:
- class WeissHapticSensorTestIn
- {
- private:
- WeissHapticSensorTestGeneratedBase<StateType>* parent;
-
- public:
- WeissHapticSensorTestIn(WeissHapticSensorTestGeneratedBase<StateType>* parent)
- : parent(parent)
- {
- }
- }; // class WeissHapticSensorTestIn
-
- class WeissHapticSensorTestLocal
- {
- private:
- WeissHapticSensorTestGeneratedBase<StateType>* parent;
-
- public:
- WeissHapticSensorTestLocal(WeissHapticSensorTestGeneratedBase<StateType>* parent)
- : parent(parent)
- {
- }
-
- public:
- std::map<std::string, ::armarx::DatafieldRefPtr> getTactileDatafields_MaximumValue() const
- {
- return parent->State::getLocal< ::armarx::StringValueMap>("TactileDatafields_MaximumValue")->::armarx::StringValueMap::toStdMap< ::armarx::DatafieldRefPtr>();
- }
- void setTactileDatafields_MaximumValue(const std::map<std::string, ::armarx::DatafieldRefPtr>& value) const
- {
- ::armarx::StringValueMapPtr container = ::armarx::StringValueMap::FromStdMap< ::armarx::DatafieldRefPtr>(value);
- parent->State::setLocal("TactileDatafields_MaximumValue", *container);
- }
- bool isTactileDatafields_MaximumValueSet() const
- {
- return parent->State::isLocalParameterSet("TactileDatafields_MaximumValue");
- }
- }; // class WeissHapticSensorTestLocal
-
- class WeissHapticSensorTestOut
- {
- private:
- WeissHapticSensorTestGeneratedBase<StateType>* parent;
-
- public:
- WeissHapticSensorTestOut(WeissHapticSensorTestGeneratedBase<StateType>* parent)
- : parent(parent)
- {
- }
- }; // class WeissHapticSensorTestOut
-
- protected:
- const WeissHapticSensorTestIn in;
- const WeissHapticSensorTestLocal local;
- const WeissHapticSensorTestOut out;
-
- public:
- WeissHapticSensorTestGeneratedBase(const XMLStateConstructorParams& stateData)
- : XMLStateTemplate < StateType > (stateData),
- in(WeissHapticSensorTestIn(this)),
- local(WeissHapticSensorTestLocal(this)),
- out(WeissHapticSensorTestOut(this))
- {
- }
- WeissHapticSensorTestGeneratedBase(const WeissHapticSensorTestGeneratedBase& source)
- : IceUtil::Shared(source),
- armarx::StateIceBase(source),
- armarx::StateBase(source),
- armarx::StateController(source),
- armarx::State(source),
- XMLStateTemplate < StateType > (source),
- in(WeissHapticSensorTestIn(this)),
- local(WeissHapticSensorTestLocal(this)),
- out(WeissHapticSensorTestOut(this))
- {
- }
-
- public:
- HapticUnitObserverInterfacePrx getHapticObserver() const
- {
- return StateBase::getContext<WeissHapticGroupStatechartContext>()->getHapticObserver();
- }
- WeissHapticUnitInterfacePrx getWeissHapticUnit() const
- {
- return StateBase::getContext<WeissHapticGroupStatechartContext>()->getWeissHapticUnit();
- }
- static std::string GetName()
- {
- return "WeissHapticSensorTest";
- }
- void __forceLibLoading()
- {
- // Do not call this method.
- // The sole purpose of this method is to force the compiler/linker to include all libraries.
- ::armarx::DatafieldRef type1;
- }
- }; // class WeissHapticSensorTestGeneratedBase
- } // namespace WeissHapticGroup
-} // namespace armarx
-
-#endif // _ARMARX_XMLUSERCODE_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICSENSORTESTGENERATEDBASE_H
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
index 8a9424eead0f7aeccd817fd54b23a14557c0f1fa..81cd19516c6e2eb7ceb6c55147de97c8207ee344 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
@@ -21,17 +21,17 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
#ifndef _ARMARX_XMLUSERCODE_RobotAPI_WeissHapticGroup_WeissHapticSensorTest_H
#define _ARMARX_XMLUSERCODE_RobotAPI_WeissHapticGroup_WeissHapticSensorTest_H
-#include "WeissHapticSensorTest.generated.h"
+#include <RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h>
namespace armarx
{
namespace WeissHapticGroup
{
- class WeissHapticSensorTest :
+ class WeissHapticSensorTest
+ :
public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
{
public:
@@ -43,7 +43,7 @@ namespace armarx
void onBreak();
void onExit();
- // static functions for AbstractFactory Method
+ // // static functions for AbstractFactory Method
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
static SubClassRegistry Registry;
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
index cb8448c7c9287cf7c959ee0243eaffecc49df3da..6e9d45bd33d9b1df4bac077169a9fdc5f01ca9a9 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
@@ -10,3 +10,4 @@
<Transitions/>
</State>
+