diff --git a/scenarios/SickLaserUnit/SickLaserUnit.scx b/scenarios/SickLaserUnit/SickLaserUnit.scx
deleted file mode 100644
index 398cd914600aeaf8faf8147dc2feb83b989d70e0..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnit/SickLaserUnit.scx
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<scenario name="SickLaserUnit" creation="2021-06-25.04:20:13 PM" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="SickLaserUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
-</scenario>
-
diff --git a/scenarios/SickLaserUnit/config/SickLaserUnit.cfg b/scenarios/SickLaserUnit/config/SickLaserUnit.cfg
deleted file mode 100644
index 1a54fe66a787a3e000d81f934a6ec0b011e1e0f5..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnit/config/SickLaserUnit.cfg
+++ /dev/null
@@ -1,293 +0,0 @@
-# ==================================================================
-# SickLaserUnit 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: yes
-# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
-
-
-# ArmarX.ApplicationName: Application name
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ApplicationName = ""
-
-
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
-# Attributes:
-# - Default: mongo/.cache
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.CachePath = mongo/.cache
-
-
-# ArmarX.Config: Comma-separated list of configuration files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Config = ""
-
-
-# ArmarX.DataPath: Semicolon-separated search list for data files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - 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: yes
-# - 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: yes
-# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-
-
-# ArmarX.DisableLogging: Turn logging off in whole application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
-
-
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
-
-
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoadLibraries = ""
-
-
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoggingGroup = ""
-
-
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
-
-
-# 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: yes
-# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
-
-
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SecondsStartupDelay = 0
-
-
-# ArmarX.SickLaserUnit.EnableProfiling: enable profiler which is used for logging performance events
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.EnableProfiling = false
-
-
-# ArmarX.SickLaserUnit.MinimumLoggingLevel: Local logging level only for this component
-# Attributes:
-# - Default: Undefined
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.SickLaserUnit.MinimumLoggingLevel = Undefined
-
-
-# ArmarX.SickLaserUnit.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.ObjectName = ""
-
-
-# ArmarX.SickLaserUnit.deviceNumber: number of the LaserScanner Device
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.deviceNumber = 0
-
-
-# ArmarX.SickLaserUnit.emulateSensor: overwrite the default Settings and don't connect to Scanner
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.emulateSensor = false
-
-
-# ArmarX.SickLaserUnit.hostname: Hostname of the LaserScanner
-# Attributes:
-# - Default: 192.168.8.133
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.SickLaserUnit.hostname = 192.168.8.133
-
-
-# ArmarX.SickLaserUnit.newIpAddress: New IP address for the LaserScanner
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.newIpAddress = ""
-
-
-# ArmarX.SickLaserUnit.port: port to use on the LaserScanner
-# Attributes:
-# - Default: 2112
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.SickLaserUnit.port = 2112
-
-
-# ArmarX.SickLaserUnit.protocol: Either use ASCII or Binary protocol
-# Attributes:
-# - Default: Binary
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {ASCII, Binary}
-ArmarX.SickLaserUnit.protocol = ASCII
-
-
-# ArmarX.SickLaserUnit.rangeMax: maximum Range of the Scanner
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.rangeMax = 1.00
-
-
-# ArmarX.SickLaserUnit.rangeMin: minimum Range of the Scanner
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.rangeMin = 0.01
-
-
-# ArmarX.SickLaserUnit.scannerType: Name of the LaserScanner
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
-
-
-# ArmarX.SickLaserUnit.sopasProtocolType: Automatically set to true if the Scanner does not support ASCII communication
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.sopasProtocolType = false
-
-
-# ArmarX.SickLaserUnit.subscribeDatagram: subscribe to Datagram in communication or not
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.subscribeDatagram = false
-
-
-# ArmarX.SickLaserUnit.timeIncrement: timeIncrement??
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.timeIncrement = 0
-
-
-# ArmarX.SickLaserUnit.timelimit: timelimit for communication
-# Attributes:
-# - Default: 5
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.SickLaserUnit.timelimit = 5
-
-
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
-
-
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
-# Attributes:
-# - Default: 1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ThreadPoolSize = 1
-
-
-# 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: yes
-# - Required: no
-# ArmarX.TopicSuffix = ""
-
-
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
-
-
-# ArmarX.Verbosity: Global logging level for whole application
-# Attributes:
-# - Default: Info
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
-
-
diff --git a/scenarios/SickLaserUnit/config/global.cfg b/scenarios/SickLaserUnit/config/global.cfg
deleted file mode 100644
index ede51536f872c9c144104e26b4754f6a0ca7b076..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnit/config/global.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-# ==================================================================
-# Global Config from Scenario SickLaserUnit
-# ==================================================================
-
diff --git a/scenarios/SickLaserUnitTest/SickLaserUnitTest.scx b/scenarios/SickLaserUnitTest/SickLaserUnitTest.scx
deleted file mode 100644
index 8fdb28bdf069b4ec5ffd5d91cffb7b38d19d5a1f..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnitTest/SickLaserUnitTest.scx
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<scenario name="SickLaserUnitTest" creation="2021-06-17.04:54:46 PM" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="SickLaserUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
-</scenario>
-
diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
deleted file mode 100644
index b988f3b13d77676d1b3a85206cf37013b3ea36a4..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ /dev/null
@@ -1,330 +0,0 @@
-# ==================================================================
-# SickLaserUnit 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: yes
-# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
-
-
-# ArmarX.ApplicationName: Application name
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ApplicationName = ""
-
-
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
-# Attributes:
-# - Default: mongo/.cache
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.CachePath = mongo/.cache
-
-
-# ArmarX.Config: Comma-separated list of configuration files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Config = ""
-
-
-# ArmarX.DataPath: Semicolon-separated search list for data files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - 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: yes
-# - 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: yes
-# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-
-
-# ArmarX.DisableLogging: Turn logging off in whole application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
-
-
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
-
-
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoadLibraries = ""
-
-
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoggingGroup = ""
-
-
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
-
-
-# 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: yes
-# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
-
-
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SecondsStartupDelay = 0
-
-
-# ArmarX.SickLaserUnit.EnableProfiling: enable profiler which is used for logging performance events
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.EnableProfiling = false
-
-
-# ArmarX.SickLaserUnit.MinimumLoggingLevel: Local logging level only for this component
-# Attributes:
-# - Default: Undefined
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.SickLaserUnit.MinimumLoggingLevel = Undefined
-
-
-# ArmarX.SickLaserUnit.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.ObjectName = ""
-
-
-# ArmarX.SickLaserUnit.TopicName: Name of the laserscanner topic to report to.
-# Attributes:
-# - Default: LaserScans
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.TopicName = LaserScans
-
-
-# ArmarX.SickLaserUnit.angleOffset: No Description
-# Attributes:
-# - Default: 0.0
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.angleOffset = 0.0
-
-
-# ArmarX.SickLaserUnit.devices: List of Devices in format frame1,ip1,port1;frame2,ip2,port2
-# Attributes:
-# - Default: LaserScannerFront,192.168.8.133,2112
-# - Case sensitivity: yes
-# - Required: no
-ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112;LaserScannerBack,192.168.8.133,2112
-
-
-# ArmarX.SickLaserUnit.heartbeat.TopicName: Name of the topic the DebugObserver listens on
-# Attributes:
-# - Default: DebugObserver
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.heartbeat.TopicName = DebugObserver
-
-
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS: TODO: maximumCycleTimeErrorMS
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS = ::_NOT_SET_::
-
-
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS: TODO: maximumCycleTimeWarningMS
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS = ::_NOT_SET_::
-
-
-# ArmarX.SickLaserUnit.heartbeatErrorMS: maximum cycle time before heartbeat Error
-# Attributes:
-# - Default: 800
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.heartbeatErrorMS = 800
-
-
-# ArmarX.SickLaserUnit.heartbeatWarnMS: maximum cycle time before heartbeat Warning
-# Attributes:
-# - Default: 500
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.heartbeatWarnMS = 500
-
-
-# ArmarX.SickLaserUnit.hostname: No Description
-# Attributes:
-# - Default: 192.168.8.133
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.hostname = 192.168.8.133
-
-
-# ArmarX.SickLaserUnit.laserScannerTopicName: No Description
-# Attributes:
-# - Default: "SickTopic"
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
-
-
-# ArmarX.SickLaserUnit.port: No Description
-# Attributes:
-# - Default: 2112
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.port = 2112
-
-
-# ArmarX.SickLaserUnit.protocol: No Description
-# Attributes:
-# - Default: ASCII
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.protocol = ASCII
-
-
-# ArmarX.SickLaserUnit.rangeMax: maximum Range of the Scanner
-# Attributes:
-# - Default: 10
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.rangeMax = 10
-
-
-# ArmarX.SickLaserUnit.rangeMin: minimum Range of the Scanner
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.rangeMin = 0
-
-
-# ArmarX.SickLaserUnit.scannerType: Name of the LaserScanner
-# Attributes:
-# - Default: sick_tim_5xx
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
-
-
-# ArmarX.SickLaserUnit.timeIncrement: No Description
-# Attributes:
-# - Default: 0.1
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.timeIncrement = 0.1
-
-
-# ArmarX.SickLaserUnit.timelimit: timelimit for communication
-# Attributes:
-# - Default: 5
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SickLaserUnit.timelimit = 5
-
-
-# ArmarX.SickLaserUnit.updatePeriod: No Description
-# Attributes:
-# - Default: 1
-# - Case sensitivity: no
-# - Required: no
-ArmarX.SickLaserUnit.updatePeriod = 1
-
-
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
-
-
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
-# Attributes:
-# - Default: 1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ThreadPoolSize = 1
-
-
-# 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: yes
-# - Required: no
-# ArmarX.TopicSuffix = ""
-
-
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
-
-
-# ArmarX.Verbosity: Global logging level for whole application
-# Attributes:
-# - Default: Info
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
-
-
diff --git a/scenarios/SickLaserUnitTest/config/global.cfg b/scenarios/SickLaserUnitTest/config/global.cfg
deleted file mode 100644
index 7772a85fc1250ba7cdb93d22b93c17d92759d93f..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnitTest/config/global.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-# ==================================================================
-# Global Config from Scenario SickLaserUnitTest
-# ==================================================================
-
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 1987708750187cc7a7e212b6c2c105157707b6ff..5cffdc6d755be9696f23479a64b4077380228046 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -543,15 +543,7 @@ namespace armarx
}
SoSeparator* newS = new SoSeparator;
- Eigen::Matrix4f lp1 = Eigen::Matrix4f::Identity();
- lp1(0, 3) = d.p1.x();
- lp1(1, 3) = d.p1.y();
- lp1(2, 3) = d.p1.z();
- Eigen::Matrix4f lp2 = Eigen::Matrix4f::Identity();
- lp2(0, 3) = d.p2.x();
- lp2(1, 3) = d.p2.y();
- lp2(2, 3) = d.p2.z();
- newS->addChild(CoinVisualizationFactory::createCoinLine(lp1, lp2, d.scale, d.color.r, d.color.g, d.color.b));
+ newS->addChild(CoinVisualizationFactory::createCoinLine(d.p1, d.p2, d.scale, d.color));
layer.addedLineVisualizations[d.name] = newS;
layer.mainNode->addChild(newS);
ARMARX_DEBUG << "drawLine2" << flush;
@@ -872,10 +864,7 @@ namespace armarx
auto node = CoinVisualizationFactory().createCircleArrow(d.radius,
d.width,
d.circleCompletion,
- d.color.r,
- d.color.g,
- d.color.b,
- d.color.transparency,
+ d.color,
16, 30);
SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
circle->setName(SELECTION_NAME(d.layerName, d.name));
@@ -2946,5 +2935,3 @@ namespace armarx
removeCircleVisu(DEBUG_LAYER_NAME, circleName, c);
}
}
-
-
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index f8de74539d68dfc44a556fb478d97f75ccaf22d6..731765e4bbcbdcb597b34f5fb6de5a5b9ae7b0ed 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -7,4 +7,3 @@ add_subdirectory(GamepadUnit)
add_subdirectory(MetaWearIMU)
add_subdirectory(KITProstheticHandDriver)
add_subdirectory(KITProsthesisIceDriver)
-add_subdirectory(SickLaserUnit)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
deleted file mode 100644
index 2c723f426a268f2a9e01aee60c3a39acee225bd6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ /dev/null
@@ -1,91 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-
-set(LIB_NAME SickLaserUnit)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-
-# If your component needs a special ice interface, define it here:
-# armarx_add_component_interface_lib(
-# SLICE_FILES
-# SickLaserUnit.ice
-# ICE_LIBS
-# # RobotAPI
-#)
-
-
-find_package(sick_scan_base)
-armarx_build_if(sick_scan_base_FOUND "sick_scan_base not available")
-
-find_package(Boost COMPONENTS system filesystem thread REQUIRED)
-armarx_build_if(Boost_FOUND "Boost not available")
-
-# Add the component
-armarx_add_component(
- COMPONENT_LIBS
- # ArmarXCore
- ArmarXCore
- ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
- # ArmarXGui
- ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
- # RobotAPI
- RobotAPICore
- ## RobotAPIInterfaces
- RobotAPIComponentPlugins # For ArViz and other plugins.
- armem_laser_scans
- LaserScansMemory # component
-
- # This project
- ## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
- # This component
- ## SickLaserUnitInterfaces # If you defined a component ice interface above.
-
- sick_scan_base::sick_scan_generic
- ${Boost_LIBRARIES}
-
- SOURCES
- SickLaserUnit.cpp
- SickScanAdapter.cpp
-
- HEADERS
- SickLaserUnit.h
- SickScanAdapter.h
-)
-
-
-# Add dependencies
-if(sick_scan_base_FOUND)
- target_include_directories(${LIB_NAME} SYSTEM PUBLIC ${sick_scan_base_INCLUDE_DIRS})
- target_compile_definitions(${LIB_NAME} PUBLIC "-Dlinux")
-endif()
-
-if(Boost_FOUND)
- target_include_directories(${LIB_NAME} PUBLIC ${Boost_INCLUDE_DIR})
-endif()
-
-# All target_include_directories must be guarded by if(Xyz_FOUND)
-# For multiple libraries write: if(X_FOUND AND Y_FOUND) ...
-#if(MyLib_FOUND)
-# target_include_directories(SickLaserUnit PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-
-# Add ARON files
-#armarx_enable_aron_file_generation_for_target(
-# TARGET_NAME
-# ${ARMARX_COMPONENT_NAME}
-# ARON_FILES
-# aron/ExampleData.xml
-#)
-
-
-# Add unit tests
-add_subdirectory(test)
-#add_subdirectory(thirdparty/sick_scan_base)
-
-# Generate the application
-armarx_generate_and_add_component_executable(
- # If your component is not defined in ::armarx, specify its namespace here:
- # COMPONENT_NAMESPACE "armarx::mynamespace"
-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/README.md b/source/RobotAPI/drivers/SickLaserUnit/README.md
deleted file mode 100644
index decfdadc6dee52122631a84d7365f4d5a7e38129..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/README.md
+++ /dev/null
@@ -1,33 +0,0 @@
-# SickLaserUnit
-Armarx adapter to the SICK laser scanner driver
-
-Uses the sick_scan_base driver to communicate with the SICK_TiM571 Laser Scanners installed in Armar-DE and translates their timestamps and data to ArmarX types.
-After conversion, the scan data is published to the configured topic.
-
-
-## Installation
-
-1. Download and build the sick_scan_base project from https://github.com/SICKAG/sick_scan_base
- ```console
- git clone https://github.com/SICKAG/sick_scan_base.git
-
- cd sick_scan_base
- mkdir -p ./build
- cd ./build
- cmake .. && make
- ```
-
-2. The package uses the script RobotAPI/etc/cmake/Findsick_scan_base.cmake to link against the sick drivers.
- For this to work, the CMake variable sick_scan_base_DIR must be set to the path where the driver has been downloaded to.
-
-3. Build this package and configure its parameters to match the Laser Scanner setup.
-
-## Runnig
-
-To use the package, make sure your LaserScanners are connected to the PC and the configured IP addresses match the ones set by the SOPAS tool.
-
-You can use ping to check whether the scanners are connected.
-```console
-ping 192.168.8.133
-```
-
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
deleted file mode 100644
index 23bb88f2d0a433961d8f88e41bf4d496698571c3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ /dev/null
@@ -1,291 +0,0 @@
-/*
- * 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::SickLaserUnit
- * @author Johann Mantel ( j-mantel at gmx dot net )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "SickLaserUnit.h"
-
-#include <exception>
-
-#include "RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h"
-
-// Include headers you only need in function definitions in the .cpp.
-
-namespace armarx
-{
-
- void
- SickLaserScanDevice::run()
- {
- while (!task->isStopped())
- {
- switch (runState)
- {
- case RunState::scannerInit:
- if (initCnt < 5)
- {
- initCnt++;
- initScanner();
- }
- else
- {
- ARMARX_WARNING
- << "Maximum number of reinitializations reached - going to idle state";
- runState = RunState::scannerFinalize;
- }
- break;
- case RunState::scannerRun:
- if (result == sick_scan::ExitSuccess) // OK -> loop again
- {
- scanData.clear();
- result = scanner->loopOnce(scanData, scanTime, scanInfo, false);
-
- // don't send out empty laser scans
- if(scanData.empty())
- {
- break;
- }
-
- if (scanTopic)
- {
- TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(
- scannerName, scannerName, scanData, scanT);
- laserScanWriter->storeSensorData(
- scanData, scannerName, robotName, scanTime);
- //trigger heartbeat
- scannerHeartbeat->heartbeat(scannerName);
- }
- else
- {
- ARMARX_WARNING << "No scan topic available: IP: " << ip
- << ", Port: " << port;
- }
- }
- else
- {
- runState = RunState::scannerInit;
- }
- break;
- case RunState::scannerFinalize:
- ARMARX_WARNING << "Scanner offline - stopping task.";
- this->task->stop();
- break;
- default:
- ARMARX_WARNING << "Invalid run state in task loop";
- break;
- }
- }
- }
-
- void
- SickLaserScanDevice::initScanner()
- {
- ARMARX_INFO_S << "Start initialising scanner " << scannerName << " [Ip: " << this->ip
- << "] [Port: " << this->port << "]";
- // attempt to connect/reconnect
- if (this->scanner)
- {
- ARMARX_WARNING_S << "Scanner " << ip << " already initialized - reinit.";
- this->scanner.reset(); // disconnect scanner
- }
- this->scanner = std::make_unique<SickScanAdapter>(
- this->ip, this->port, this->timelimit, this->parser.get(), 'A');
- this->result = this->scanner->init();
-
- if (this->result == sick_scan::ExitSuccess)
- {
- ARMARX_INFO << "Reading scanner " << ip;
- //read the scanner parameters for initialization
- this->result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
- ARMARX_INFO << "Received result from scanner " << ip;
-
- }
- if (this->result == sick_scan::ExitSuccess) // OK -> loop again
- {
- ARMARX_INFO_S << "Scanner `" << ip << "` successfully initialized.";
- this->runState = RunState::scannerRun; // after initialising switch to run state
- }
- else
- {
- this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
- }
- }
-
-
- armarx::PropertyDefinitionsPtr
- SickLaserUnit::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr def =
- new ComponentPropertyDefinitions(getConfigIdentifier());
- // Publish to a topic (passing the TopicListenerPrx).
- // def->topic(myTopicListener);
-
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- // def->component(myComponentProxy)
-
- def->topic(topic,
- properties.topicName,
- "TopicName",
- "Name of the laserscanner topic to report to.");
- //Scanner parameters
- def->optional(properties.devices,
- "devices",
- "List of Devices in format frame1,ip1,port1;frame2,ip2,port2");
- def->optional(properties.scannerType, "scannerType", "Name of the LaserScanner");
- def->optional(properties.timelimit, "timelimit", "timelimit for communication");
- def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
- def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
-
- def->optional(properties.robotName, "robotName", "");
- return def;
- }
-
- SickLaserUnit::SickLaserUnit()
- {
- addPlugin(heartbeat);
- ARMARX_CHECK_NOT_NULL(heartbeat);
-
- addPlugin(laserScanWriterPlugin);
-
- // {
- // Ice::PropertiesPtr properties = getIceProperties()->clone();
-
- // const std::string configName = "sensory_memory";
-
- // IceInternal::Handle<armem::server::laser_scans::LaserScansMemory> sensoryMemory =
- // Component::create<armem::server::laser_scans::LaserScansMemory>(
- // properties, configName, getConfigDomain());
- // getArmarXManager()->addObjectAsync(sensoryMemory, "", true, false);
- // }
- }
-
- void
- SickLaserUnit::onInitComponent()
- {
- ARMARX_INFO << "On init";
- // Topics and properties defined above are automagically registered.
- //offeringTopic(properties.topicName);
-
- // Keep debug observer data until calling `sendDebugObserverBatch()`.
- // (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
-
- std::vector<std::string> splitDeviceStrings = Split(properties.devices, ";");
- scanDevices.clear();
- scanDevices.reserve(splitDeviceStrings.size());
- for (std::string const& deviceString : splitDeviceStrings)
- {
- std::vector<std::string> deviceInfo = Split(deviceString, ",");
- if (deviceInfo.size() != 3)
- {
- ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
- << " (split size: " << deviceInfo.size() << ", expected: 3)";
- continue;
- }
- SickLaserScanDevice& device = scanDevices.emplace_back();
- device.scannerName = deviceInfo[0];
- device.ip = deviceInfo[1];
- device.port = deviceInfo[2];
- device.timelimit = properties.timelimit;
- //scanInfo
- device.scanInfo.device = device.ip;
- device.scanInfo.frame = device.scannerName;
-
- device.robotName = properties.robotName;
- //scanner Parameters
- try
- {
- device.parser =
- std::make_unique<sick_scan::SickGenericParser>(properties.scannerType);
- device.parser->set_range_min(properties.rangeMin);
- device.parser->set_range_max(properties.rangeMax);
- device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- }
- catch (std::exception const& e)
- {
- ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
- return;
- }
- device.scannerHeartbeat = heartbeat;
- device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName,
- "No LaserScan data available");
- }
- }
-
- void
- SickLaserUnit::onConnectComponent()
- {
- for (SickLaserScanDevice& device : scanDevices)
- {
- device.scanTopic = topic;
- device.laserScanWriter = &laserScanWriterPlugin->get();
- //start the laser scanner
- if (device.task)
- {
- ARMARX_WARNING << "this should not happen.";
- device.task->stop();
- device.task = nullptr;
- }
- device.runState = RunState::scannerInit;
- device.task = new RunningTask<SickLaserScanDevice>(
- &device, &SickLaserScanDevice::run, "SickLaserScanUpdate_" + device.ip);
- device.task->start();
- }
- // Do things after connecting to topics and components.
-
- /* (Requies the armarx::DebugObserverComponentPluginUser.)
- // Use the debug observer to log data over time.
- // The data can be viewed in the ObserverView and the LivePlotter.
- // (Before starting any threads, we don't need to lock mutexes.)
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- */
- }
-
- void
- SickLaserUnit::onDisconnectComponent()
- {
- ARMARX_INFO_S << "Disconnecting LaserScanner.";
- for (SickLaserScanDevice& device : scanDevices)
- {
- if (device.task)
- {
- device.task->stop();
- device.task = nullptr;
- }
- }
- }
-
- void
- SickLaserUnit::onExitComponent()
- {
- }
-
- std::string
- SickLaserUnit::getDefaultName() const
- {
- return "SickLaserUnit";
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
deleted file mode 100644
index 1fed1aab9d05370280535c4314d9a4104ce25fcb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
- * 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::SickLaserUnit
- * @author Johann Mantel ( j-mantel at gmx dot net )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// #include <mutex>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include "RobotAPI/libraries/armem_laser_scans/client/common/Writer.h"
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-
-#include <vector>
-
-#include "SickScanAdapter.h"
-#include <sick_scan/sick_scan_common.h>
-#include <sick_scan/sick_scan_common_tcp.h>
-#include <sick_scan/sick_generic_laser.h>
-
-namespace armarx
-{
-
- enum class ScanProtocol
- {
- ASCII,
- Binary
- };
-
- enum class RunState
- {
- scannerInit,
- scannerRun,
- scannerFinalize
- };
-
- struct SickLaserScanDevice
- {
- std::string scannerName;
- //communication parameters
- std::string ip;
- std::string port;
- int timelimit = 5;
- //data and task pointers
- IceUtil::Time scanTime;
- LaserScan scanData;
- LaserScannerInfo scanInfo;
- int initCnt = 0;
- int result = sick_scan::ExitError;
- RunState runState = RunState::scannerFinalize;
- RunningTask<SickLaserScanDevice>::pointer_type task;
- LaserScannerUnitListenerPrx scanTopic;
- plugins::HeartbeatComponentPlugin* scannerHeartbeat;
-
- armarx::armem::laser_scans::client::Writer* laserScanWriter;
-
- std::string robotName;
- //scanner pointers
- std::unique_ptr<sick_scan::SickGenericParser> parser;
- std::unique_ptr<SickScanAdapter> scanner;
-
- void initScanner();
- void run();
- };
- /**
- * @defgroup Component-SickLaserUnit SickLaserUnit
- * @ingroup RobotAPI-Components
- * A description of the component SickLaserUnit.
- *
- * @class SickLaserUnit
- * @ingroup Component-SickLaserUnit
- * @brief Brief description of class SickLaserUnit.
- *
- * Detailed description of class SickLaserUnit.
- */
- class SickLaserUnit :
- //virtual public armarx::LaserScannerUnitInterface,
- virtual public armarx::Component
- // , virtual public armarx::RobotHealthComponentUser
- // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
- {
- public:
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
- SickLaserUnit();
-
- protected:
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- /// @see armarx::ManagedIceObject::onInitComponent()
- void onInitComponent() override;
-
- /// @see armarx::ManagedIceObject::onConnectComponent()
- void onConnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
- void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
- void onExitComponent() override;
-
- private:
- // Private methods go here.
-
- private:
- // Private member variables go here.
-
- /// Properties shown in the Scenario GUI.
- struct Properties
- {
- std::string topicName = "LaserScans";
- //scanner parameters
- std::string devices = "LaserScannerFront,192.168.8.133,2112";
- std::string scannerType = "sick_tim_5xx";
-
- int timelimit = 5;
-
- double rangeMin = 0.0;
- double rangeMax = 12.0;
-
- std::string robotName = "Armar6";
- };
- Properties properties;
- std::vector<SickLaserScanDevice> scanDevices;
- LaserScannerUnitListenerPrx topic;
- plugins::HeartbeatComponentPlugin* heartbeat = nullptr;
-
- armem::client::plugins::ReaderWriterPlugin<armarx::armem::laser_scans::client::Writer>*
- laserScanWriterPlugin = nullptr;
-
- };
-} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
deleted file mode 100644
index 3d20aad67f1a121c641bfa4f82021c9d2a59d74b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ /dev/null
@@ -1,1002 +0,0 @@
-/**
-* \file
-* \brief Laser Scanner communication (TCP Helper Class)
-* Copyright (C) 2013, Osnabrück University
-* Copyright (C) 2017, Ing.-Buero Dr. Michael Lehning, Hildesheim
-* Copyright (C) 2017, SICK AG, Waldkirch
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above copyright
-* notice, this list of conditions and the following disclaimer in the
-* documentation and/or other materials provided with the distribution.
-* * Neither the name of Osnabrück University nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission.
-* * Neither the name of SICK AG nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission
-* * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Last modified: 12th Dec 2017
-*
-* Authors:
-* Michael Lehning <michael.lehning@lehning.de>
-* Jochen Sprickerhof <jochen@sprickerhof.de>
-* Martin Günther <mguenthe@uos.de>
-*
-* Based on the TiM communication example by SICK AG.
-*
-*/
-
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include <VirtualRobot/MathTools.h>
-#ifdef _MSC_VER
-#pragma warning(disable: 4996)
-#pragma warning(disable: 4267)
-#pragma warning(disable: 4101) // C4101: "e" : Unreferenzierte lokale Variable
-#define _WIN32_WINNT 0x0501
-
-#endif
-
-#include "SickScanAdapter.h"
-#include <sick_scan/tcp/colaa.hpp>
-#include <sick_scan/tcp/colab.hpp>
-#include <sick_scan/sick_generic_radar.h>
-#include <sick_scan/helper/angle_compensator.h>
-#include <sick_scan/sick_scan_config_internal.h>
-#include <sick_scan/sick_generic_imu.h>
-
-#ifdef ROSSIMU
-#include <sick_scan/rosconsole_simu.hpp>
-#endif
-
-#include <sick_scan/binScanf.hpp>
-#include <sick_scan/RadarScan.h>
-#include <sick_scan/Encoder.h>
-
-#include <boost/asio.hpp>
-#include <boost/lambda/lambda.hpp>
-#include <algorithm>
-#include <iterator>
-#include <boost/lexical_cast.hpp>
-#include <vector>
-
-// #ifndef rad2deg
-// #define rad2deg(x) ((x) / M_PI * 180.0)
-// #endif
-// #define deg2rad_const (0.017453292519943295769236907684886f)
-
-
-//std::vector<unsigned char> exampleData(65536);
-//std::vector<unsigned char> receivedData(65536);
-//static long receivedDataLen = 0;
-
-//static int getDiagnosticErrorCode()
-//{
-//#ifdef _MSC_VER
-//#undef ERROR
-// return (2);
-//#else
-// return (diagnostic_msgs::DiagnosticStatus::ERROR);
-//#endif
-//}
-
-namespace armarx
-{
- SickScanAdapter::SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit,
- sick_scan::SickGenericParser* parser, char cola_dialect_id)
- :
- sick_scan::SickScanCommon(parser),
- socket_(io_service_),
- deadline_(io_service_),
- hostname_(hostname),
- port_(port),
- timelimit_(timelimit)
- {
- if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
- {
- this->setProtocolType(CoLa_A);
- }
-
- if ((cola_dialect_id == 'b') || (cola_dialect_id == 'B'))
- {
- this->setProtocolType(CoLa_B);
- }
- ARMARX_CHECK(this->getProtocolType() != CoLa_Unknown);
-
- m_numberOfBytesInReceiveBuffer = 0;
- m_alreadyReceivedBytes = 0;
- this->setReplyMode(0);
- // io_service_.setReadCallbackFunction(boost::bind(&SopasDevice::readCallbackFunction, this, _1, _2));
-
- // Set up the deadline actor to implement timeouts.
- // Based on blocking TCP example on:
- // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
- deadline_.expires_at(boost::posix_time::pos_infin);
- checkDeadline();
- }
-
- SickScanAdapter::~SickScanAdapter()
- {
- //stopScanData();
- close_device();
- }
-
- using boost::asio::ip::tcp;
- using boost::lambda::var;
- using boost::lambda::_1;
-
-
- /*!
- \brief parsing datagram into ARMARX message
- \return error code
- */
- int SickScanAdapter::loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo)
- {
- unsigned char receiveBuffer[65536];
- int actual_length = 0;
- int packetsInLoop = 0;
-
- //always use ASCII
- int result = getDatagramIceTime(scanTime, receiveBuffer, &actual_length, &packetsInLoop);
- //ros::Duration dur = recvTimeStampPush - recvTimeStamp;
- if (result != 0)
- {
- ARMARX_ERROR_S << "Read Error when getting datagram: " << result;
- return sick_scan::ExitError; // return failure to exit node
- }
- if (actual_length <= 0)
- {
- return sick_scan::ExitSuccess;
- } // return success to continue looping
-
- //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
- char* buffer_pos = (char*) receiveBuffer;
- char* dstart = NULL;
- char* dend = NULL;
- bool dataToProcess = true;
- std::vector<float> vang_vec;
- vang_vec.clear();
-
- while (dataToProcess)
- {
- size_t dlength;
- int success = -1;
- // Always Parsing Ascii-Encoding of datagram
- dstart = strchr(buffer_pos, 0x02);
- if (dstart != NULL)
- {
- dend = strchr(dstart + 1, 0x03);
- }
- if ((dstart != NULL) && (dend != NULL))
- {
- dataToProcess = true; // continue parsing
- dlength = dend - dstart;
- *dend = '\0';
- dstart++;
- }
- else
- {
- dataToProcess = false;
- break;
- }
- // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
- // <frameid>_<sign>00500_DIST[1|2|3]
- success = parseDatagram(dstart, dlength, scanData, scanInfo, updateScannerInfo);
- if (success != sick_scan::ExitSuccess)
- {
- ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
- }
- // Start Point
- if (dend != NULL)
- {
- buffer_pos = dend + 1;
- }
- } // end of while loop
- return sick_scan::ExitSuccess; // return success to continue looping
- }
-
- //Adapted from sick_generic_parser
- int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo)
- {
- int HEADER_FIELDS = 32;
- char* cur_field;
- size_t count = 0;
-
- // Reserve sufficient space
- std::vector<char*> fields;
- fields.reserve(datagram_length / 2);
- std::vector<char> datagram_copy_vec;
- datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
- char* datagram_copy = &(datagram_copy_vec[0]);
- strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
- datagram_copy[datagram_length] = 0;
-
- cur_field = strtok(datagram, " ");
- while (cur_field != NULL)
- {
- fields.push_back(cur_field);
- cur_field = strtok(NULL, " ");
- }
- count = fields.size();
-
- // Validate header. Total number of tokens is highly unreliable as this may
- // change when you change the scanning range or the device name using SOPAS ET
- // tool. The header remains stable, however.
- if ((int) count < HEADER_FIELDS)
- {
- ARMARX_ERROR_S << "received less fields than minimum fields (actual: " << count
- << ", minimum: " << HEADER_FIELDS << "), ignoring scan\n"
- << "are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)";
- return sick_scan::ExitError;
- }
-
- if (strcmp(fields[15], "0"))
- {
- ARMARX_ERROR_S << "Field 15 of received data is not equal to 0. Unexpected data, ignoring scan";
- return sick_scan::ExitError;
- }
- if (strcmp(fields[20], "DIST1"))
- {
- ARMARX_ERROR_S << "Field 20 of received data is not equal to DIST1i. Unexpected data, ignoring scan";
- return sick_scan::ExitError;
- }
-
- // More in depth checks: check data length and RSSI availability
- // 25: Number of data (<= 10F)
- unsigned short int number_of_data = 0;
- sscanf(fields[25], "%hx", &number_of_data);
-
- if ((int) count < HEADER_FIELDS + number_of_data)
- {
- ARMARX_ERROR_S << "Less fields than expected for data points - Ignoring scan";
- return sick_scan::ExitError;
- }
-
- // Calculate offset of field that contains indicator of whether or not RSSI data is included
- size_t rssi_idx = 26 + number_of_data;
- bool rssi = false;
- unsigned short int number_of_rssi_data = 0;
- if (strcmp(fields[rssi_idx], "RSSI1") == 0)
- {
- rssi = true;
- }
- if (rssi)
- {
- sscanf(fields[rssi_idx + 5], "%hx", &number_of_rssi_data);
- // Number of RSSI data should be equal to number of data
- if (number_of_rssi_data != number_of_data)
- {
- ARMARX_ERROR_S << "Number of RSSI data is lower than number of range data";
- return sick_scan::ExitError;
- }
-
- // Check if the total length is still appropriate.
- // RSSI data size = number of RSSI readings + 6 fields describing the data
- if ((int) count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
- {
- ARMARX_ERROR_S << "Less fields than expected for %d data points (%zu). Ignoring scan";
- return sick_scan::ExitError;
- }
-
- if (strcmp(fields[rssi_idx], "RSSI1"))
- {
- ARMARX_ERROR_S << "A Field of received data is not equal to RSSI1 - Unexpected data, ignoring scan";
- }
- }
-
- //IceUtil::Time start_time = IceUtil::Time::now(); // will be adjusted in the end
-
- // <STX> (\x02)
- // 0: Type of command (SN)
- // 1: Command (LMDscandata)
- // 2: Firmware version number (1)
- // 3: Device number (1)
- // 4: Serial number (eg. B96518)
- // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
- // 7: Telegram counter (eg. 99)
- // 8: Scan counter (eg. 9A)
- // 9: Time since startup (eg. 13C8E59)
- // 10: Time of transmission (eg. 13C9CBE)
- // 11 + 12: Input status (0 0)
- // 13 + 14: Output status (8 0)
- // 15: Reserved Byte A (0)
- // 16: Scanning Frequency (5DC)
- unsigned short scanning_freq = -1;
- sscanf(fields[16], "%hx", &scanning_freq);
- // msg.scan_time = 1.0 / (scanning_freq / 100.0);
-
- // 17: Measurement Frequency (36)
- unsigned short measurement_freq = -1;
- sscanf(fields[17], "%hx", &measurement_freq);
- // msg.time_increment = 1.0 / (measurement_freq * 100.0);
-
- // 18: Number of encoders (0)
- // 19: Number of 16 bit channels (1)
- // 20: Measured data contents (DIST1)
- // 21: Scaling factor (3F800000)
- // ignored for now (is always 1.0):
- // 22: Scaling offset (00000000) -- always 0
- // 23: Starting angle (FFF92230)
- if (updateScannerInfo)
- {
- scanInfo.device = hostname_;
- uint starting_angle = (uint) - 1;
- sscanf(fields[23], "%x", &starting_angle);
- scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
-
- // 24: Angular step width (2710)
- unsigned short angular_step_width = -1;
- sscanf(fields[24], "%hx", &angular_step_width);
- scanInfo.stepSize = (angular_step_width / 10000.0) / 180.0 * M_PI;
- scanInfo.maxAngle = scanInfo.minAngle + (number_of_data - 1) * scanInfo.stepSize;
- }
-
- // 25: Number of data (<= 10F)
- // This is already determined above in number_of_data
-
- int distNum = 0;
- int rssiNum = 0;
- int baseOffset = 20;
- // More in depth checks: check data length and RSSI availability
- // 25: Number of data (<= 10F)
- unsigned short int curr_number_of_data = 0;
- if (strstr(fields[baseOffset], "DIST") != fields[baseOffset]) // First initial check
- {
- ARMARX_ERROR_S << "Field 20 of received data does not start with DIST - Unexpected data, ignoring scan";
- return sick_scan::ExitError;
- }
-
- //Read either intensity or distance data, regarding what type is given in the datagram
- std::vector<float> distVal, intensityVal;
- for (int offset = 20; offset < (int) fields.size();)
- {
- bool distFnd = false;
- bool rssiFnd = false;
- if (strlen(fields[offset]) == 5)
- {
- if (strstr(fields[offset], "DIST") == fields[offset])
- {
- distNum++;
- distFnd = true;
- }
- else if (strstr(fields[offset], "RSSI") == fields[offset])
- {
- rssiNum++;
- rssiFnd = true;
- }
- }
- if (rssiFnd || distFnd)
- {
- offset += 5;
- if (offset >= (int) fields.size())
- {
- ARMARX_ERROR_S << "Missing RSSI or DIST data";
- return sick_scan::ExitError;
- }
- curr_number_of_data = 0;
- sscanf(fields[offset], "%hx", &curr_number_of_data);
- if (curr_number_of_data != number_of_data)
- {
- ARMARX_ERROR_S << "number of dist or rssi values mismatching.";
- return sick_scan::ExitError;
- }
- offset++;
- // Here is the first value
- for (int i = 0; i < curr_number_of_data; i++)
- {
- if (distFnd)
- {
- unsigned short iRange;
- sscanf(fields[offset + i], "%hx", &iRange);
- float range = iRange; // / 1000.0;
- distVal.push_back(range);
- }
- else
- {
- unsigned short iRSSI;
- sscanf(fields[offset + i], "%hx", &iRSSI);
- intensityVal.push_back((float) iRSSI);
- }
- }
- offset += number_of_data;
- }
- else
- {
- offset++;
- }
- }
-
- if (distVal.size() != intensityVal.size())
- {
- ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
- return sick_scan::ExitError;
- }
-
- // FIXME scan info is not set atm.
- scanInfo.minAngle = VirtualRobot::MathTools::deg2rad(-45);
- scanInfo.maxAngle = VirtualRobot::MathTools::deg2rad(225);
- scanInfo.stepSize = (scanInfo.maxAngle - scanInfo.minAngle) / (distVal.size()-1);
-
- ARMARX_VERBOSE << "Min/max angle: " << VirtualRobot::MathTools::rad2deg(scanInfo.minAngle) << ", " << VirtualRobot::MathTools::rad2deg(scanInfo.maxAngle) << "";
-
- scanData.reserve(distVal.size());
- for (int i = 0; i < (int) distVal.size(); i++)
- {
- LaserScanStep step;
- step.angle = i * scanInfo.stepSize + scanInfo.minAngle;
- // step.angle = scanInfo.maxAngle - i * scanInfo.stepSize;
- ARMARX_CHECK_LESS_EQUAL(step.angle, scanInfo.maxAngle);
- ARMARX_CHECK_GREATER_EQUAL(step.angle, scanInfo.minAngle);
-
- step.distance = distVal[i];
- //step.intensity = intensityVal[i];
- scanData.push_back(step);
- }
-
- // 26 + n: RSSI data included
- // IF RSSI not included:
- // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
- // 26 + n + 4 .. count - 4 = device label
- // count - 3 .. count - 1 = unknown (but seems to be 0 always)
- // <ETX> (\x03)
- return sick_scan::ExitSuccess;
- }
-
-
- void SickScanAdapter::disconnectFunction()
- {
- }
-
- void SickScanAdapter::disconnectFunctionS(void* obj)
- {
- if (obj != NULL)
- {
- ((SickScanAdapter*)(obj))->disconnectFunction();
- }
- }
-
- void SickScanAdapter::readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes)
- {
- ((SickScanAdapter*) obj)->readCallbackFunction(buffer, numOfBytes);
- }
-
-
- void SickScanAdapter::setReplyMode(int _mode)
- {
- m_replyMode = _mode;
- }
-
- int SickScanAdapter::getReplyMode()
- {
- return (m_replyMode);
- }
-
- //
- // Look for 23-frame (STX/ETX) in receive buffer.
- // Move frame to start of buffer
- //
- // Return: 0 : No (complete) frame found
- // >0 : Frame length
- //
- SopasEventMessage SickScanAdapter::findFrameInReceiveBuffer()
- {
- UINT32 frameLen = 0;
- UINT32 i;
-
- // Depends on protocol...
- if (getProtocolType() == CoLa_A)
- {
- //
- // COLA-A
- //
- // Must start with STX (0x02)
- if (m_receiveBuffer[0] != 0x02)
- {
- // Look for starting STX (0x02)
- for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
- {
- if (m_receiveBuffer[i] == 0x02)
- {
- break;
- }
- }
-
- // Found beginning of frame?
- if (i >= m_numberOfBytesInReceiveBuffer)
- {
- // No start found, everything can be discarded
- m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
- return SopasEventMessage(); // No frame found
- }
-
- // Move frame start to index 0
- UINT32 newLen = m_numberOfBytesInReceiveBuffer - i;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), newLen);
- m_numberOfBytesInReceiveBuffer = newLen;
- }
-
- // Look for ending ETX (0x03)
- for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
- {
- if (m_receiveBuffer[i] == 0x03)
- {
- break;
- }
- }
-
- // Found end?
- if (i >= m_numberOfBytesInReceiveBuffer)
- {
- // No end marker found, so it's not a complete frame (yet)
- return SopasEventMessage(); // No frame found
- }
-
- // Calculate frame length in byte
- frameLen = i + 1;
-
- return SopasEventMessage(m_receiveBuffer, CoLa_A, frameLen);
- }
- else if (getProtocolType() == CoLa_B)
- {
- UINT32 magicWord;
- UINT32 payloadlength;
-
- if (m_numberOfBytesInReceiveBuffer < 4)
- {
- return SopasEventMessage();
- }
- UINT16 pos = 0;
- magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- if (magicWord != 0x02020202)
- {
- // Look for starting STX (0x02020202)
- for (i = 1; i <= m_numberOfBytesInReceiveBuffer - 4; i++)
- {
- pos = i; // this is needed, as the position value is updated by getIntegerFromBuffer
- magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- if (magicWord == 0x02020202)
- {
- // found magic word
- break;
- }
- }
-
- // Found beginning of frame?
- if (i > m_numberOfBytesInReceiveBuffer - 4)
- {
- // No start found, everything can be discarded
- m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
- return SopasEventMessage(); // No frame found
- }
- else
- {
- // Move frame start to index
- UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - i;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), bytesToMove); // payload+magic+length+s+checksum
- m_numberOfBytesInReceiveBuffer = bytesToMove;
- }
- }
-
- // Pruefe Laenge des Pufferinhalts
- if (m_numberOfBytesInReceiveBuffer < 9)
- {
- // Es sind nicht genug Daten fuer einen Frame
- printInfoMessage("SickScanCommonNw::findFrameInReceiveBuffer: Frame cannot be decoded yet, only " +
- ::toString(m_numberOfBytesInReceiveBuffer) + " bytes in the buffer.", m_beVerbose);
- return SopasEventMessage();
- }
-
- // Read length of payload
- pos = 4;
- payloadlength = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- printInfoMessage(
- "SickScanCommonNw::findFrameInReceiveBuffer: Decoded payload length is " + ::toString(payloadlength) +
- " bytes.", m_beVerbose);
-
- // Ist die Datenlaenge plausibel und wuede in den Puffer passen?
- if (payloadlength > (sizeof(m_receiveBuffer) - 9))
- {
- // magic word + length + checksum = 9
- printWarning(
- "SickScanCommonNw::findFrameInReceiveBuffer: Frame too big for receive buffer. Frame discarded with length:"
- + ::toString(payloadlength) + ".");
- m_numberOfBytesInReceiveBuffer = 0;
- return SopasEventMessage();
- }
- if ((payloadlength + 9) > m_numberOfBytesInReceiveBuffer)
- {
- // magic word + length + s + checksum = 10
- printInfoMessage(
- "SickScanCommonNw::findFrameInReceiveBuffer: Frame not complete yet. Waiting for the rest of it (" +
- ::toString(payloadlength + 9 - m_numberOfBytesInReceiveBuffer) + " bytes missing).", m_beVerbose);
- return SopasEventMessage(); // frame not complete
- }
-
- // Calculate the total frame length in bytes: Len = Frame (9 bytes) + Payload
- frameLen = payloadlength + 9;
-
- //
- // test checksum of payload
- //
- UINT8 temp = 0;
- UINT8 temp_xor = 0;
- UINT8 checkSum;
-
- // Read original checksum
- pos = frameLen - 1;
- checkSum = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
-
- // Erzeuge die Pruefsumme zum Vergleich
- for (UINT16 i = 8; i < (frameLen - 1); i++)
- {
- pos = i;
- temp = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
- temp_xor = temp_xor ^ temp;
- }
-
- // Vergleiche die Pruefsummen
- if (temp_xor != checkSum)
- {
- printWarning("SickScanCommonNw::findFrameInReceiveBuffer: Wrong checksum, Frame discarded.");
- m_numberOfBytesInReceiveBuffer = 0;
- return SopasEventMessage();
- }
-
- return SopasEventMessage(m_receiveBuffer, CoLa_B, frameLen);
- }
-
- // Return empty frame
- return SopasEventMessage();
- }
-
-
- /**
- * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
- * hereingekommen sind.
- */
-
- void SickScanAdapter::processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame)
- {
-
- if (getProtocolType() == CoLa_A)
- {
- printInfoMessage(
- "SickScanCommonNw::processFrame: Calling processFrame_CoLa_A() with " + ::toString(frame.size()) + " bytes.",
- m_beVerbose);
- // processFrame_CoLa_A(frame);
- }
- else if (getProtocolType() == CoLa_B)
- {
- printInfoMessage(
- "SickScanCommonNw::processFrame: Calling processFrame_CoLa_B() with " + ::toString(frame.size()) + " bytes.",
- m_beVerbose);
- // processFrame_CoLa_B(frame);
- }
-
- // Push frame to recvQueue
-
- DatagramWithTimeStamp dataGramWidthTimeStamp(timeStamp, std::vector<unsigned char>(frame.getRawData(),
- frame.getRawData() +
- frame.size()));
- // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
- recvQueue.push(dataGramWidthTimeStamp);
- }
-
- void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
- {
- IceUtil::Time rcvTimeStamp = IceUtil::Time::now(); // stamp received datagram
- bool beVerboseHere = false;
-
- ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
- UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
- UINT32 bytesToBeTransferred = numOfBytes;
- if (remainingSpace < numOfBytes)
- {
- bytesToBeTransferred = remainingSpace;
- // printWarning("SickScanCommonNw::readCallbackFunction(): Input buffer space is to small, transferring only " +
- // ::toString(bytesToBeTransferred) + " of " + ::toString(numOfBytes) + " bytes.");
- }
- else
- {
- // printInfoMessage("SickScanCommonNw::readCallbackFunction(): Transferring " + ::toString(bytesToBeTransferred) +
- // " bytes from TCP to input buffer.", beVerboseHere);
- }
-
- if (bytesToBeTransferred > 0)
- {
- // Data can be transferred into our input buffer
- memcpy(&(m_receiveBuffer[m_numberOfBytesInReceiveBuffer]), buffer, bytesToBeTransferred);
- m_numberOfBytesInReceiveBuffer += bytesToBeTransferred;
-
- UINT32 size = 0;
-
- while (1)
- {
- // Now work on the input buffer until all received datasets are processed
- SopasEventMessage frame = findFrameInReceiveBuffer();
-
- size = frame.size();
- if (size == 0)
- {
- // Framesize = 0: There is no valid frame in the buffer. The buffer is either empty or the frame
- // is incomplete, so leave the loop
- printInfoMessage("SickScanCommonNw::readCallbackFunction(): No complete frame in input buffer, we are done.",
- beVerboseHere);
-
- // Leave the loop
- break;
- }
- else
- {
- // A frame was found in the buffer, so process it now.
- printInfoMessage(
- "SickScanCommonNw::readCallbackFunction(): Processing a frame of length " + ::toString(frame.size()) +
- " bytes.", beVerboseHere);
- processFrame(rcvTimeStamp, frame);
- UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - size;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[size]), bytesToMove); // payload+magic+length+s+checksum
- m_numberOfBytesInReceiveBuffer = bytesToMove;
-
- }
- }
- }
- else
- {
- // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
- // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
- // we clear the input buffer here.
- m_numberOfBytesInReceiveBuffer = 0;
- }
- }
-
-
- int SickScanAdapter::init_device()
- {
- int portInt;
- sscanf(port_.c_str(), "%d", &portInt);
- m_nw.init(hostname_, portInt, disconnectFunctionS, (void*) this);
- m_nw.setReadCallbackFunction(readCallbackFunctionS, (void*) this);
- m_nw.connect();
- return sick_scan::ExitSuccess;
- }
-
- int SickScanAdapter::close_device()
- {
- ARMARX_ERROR_S << "Disconnecting TCP-Connection.";
- m_nw.disconnect();
- return 0;
- }
-
-
- bool SickScanAdapter::stopScanData()
- {
- stop_scanner();
- return (true);
- }
-
- void SickScanAdapter::handleRead(boost::system::error_code error, size_t bytes_transfered)
- {
- ec_ = error;
- bytes_transfered_ += bytes_transfered;
- }
-
-
- void SickScanAdapter::checkDeadline()
- {
- if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
- {
- // The reason the function is called is that the deadline expired. Close
- // the socket to return all IO operations and reset the deadline
- socket_.close();
- deadline_.expires_at(boost::posix_time::pos_infin);
- }
-
- // Nothing bad happened, go back to sleep
- deadline_.async_wait(boost::bind(&SickScanAdapter::checkDeadline, this));
- }
-
-
- int SickScanAdapter::numberOfDatagramInInputFifo()
- {
- int numVal = this->recvQueue.getNumberOfEntriesInQueue();
- return (numVal);
- }
-
- int SickScanAdapter::readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read,
- bool* exception_occured, bool isBinary)
- {
- // Set up the deadline to the proper timeout, error and delimiters
- deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
-
- ec_ = boost::asio::error::would_block;
- bytes_transfered_ = 0;
-
- // Polling - should be changed to condition variable in the future
- int waitingTimeInMs = 1; // try to lookup for new incoming packages
- size_t i;
- for (i = 0; i < timeout_ms; i += waitingTimeInMs)
- {
- if (false == this->recvQueue.isQueueEmpty())
- {
- break;
- }
- boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
- }
- if (i >= timeout_ms)
- {
- ROS_ERROR("no answer received after %zu ms. Maybe sopas mode is wrong.\n", timeout_ms);
- return (sick_scan::ExitError);
- }
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
-
- *bytes_read = datagramWithTimeStamp.datagram.size();
- memcpy(buffer, &(datagramWithTimeStamp.datagram[0]), datagramWithTimeStamp.datagram.size());
- return (sick_scan::ExitSuccess);
- }
-
- /**
- * Send a SOPAS command to the device and print out the response to the console.
- */
- int SickScanAdapter::sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen)
- {
- int sLen = 0;
- int preambelCnt = 0;
- bool cmdIsBinary = false;
-
- if (request != NULL)
- {
- sLen = cmdLen;
- preambelCnt = 0; // count 0x02 bytes to decide between ascii and binary command
- if (sLen >= 4)
- {
- for (int i = 0; i < 4; i++)
- {
- if (request[i] == 0x02)
- {
- preambelCnt++;
- }
- }
- }
-
- if (preambelCnt < 4)
- {
- cmdIsBinary = false;
- }
- else
- {
- cmdIsBinary = true;
- }
- int msgLen = 0;
- if (cmdIsBinary == false)
- {
- msgLen = strlen(request);
- }
- else
- {
- int dataLen = 0;
- for (int i = 4; i < 8; i++)
- {
- dataLen |= ((unsigned char) request[i] << (7 - i) * 8);
- }
- msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
- }
-#if 1
-
- bool debugBinCmd = false;
- if (debugBinCmd)
- {
- printf("=== START HEX DUMP ===\n");
- for (int i = 0; i < msgLen; i++)
- {
- unsigned char* ptr = (UINT8*) request;
- printf("%02x ", ptr[i]);
- }
- printf("\n=== END HEX DUMP ===\n");
- }
- m_nw.sendCommandBuffer((UINT8*) request, msgLen);
-#else
-
- /*
- * Write a SOPAS variable read request to the device.
- */
- try
- {
- boost::asio::write(socket_, boost::asio::buffer(request, msgLen));
- }
- catch (boost::system::system_error& e)
- {
- ROS_ERROR("write error for command: %s", request);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "Write error for sendSOPASCommand.");
- return sick_scan::ExitError;
- }
-#endif
- }
-
- // Set timeout in 5 seconds
- const int BUF_SIZE = 65536;
- char buffer[BUF_SIZE];
- int bytes_read;
- // !!!
- if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
- {
- ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
- return sick_scan::ExitError;
- }
-
-
- if (reply)
- {
- reply->resize(bytes_read);
-
- std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
- }
- return sick_scan::ExitSuccess;
- }
-
- int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
- {
- return sick_scan::ExitError;
- }
-
- int SickScanAdapter::getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer,
- int* actual_length, int* numberOfRemainingFifoEntries)
- {
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = 0;
- }
- this->setReplyMode(1);
- const int maxWaitInMs = getReadTimeOutInMs();
- std::vector<unsigned char> dataBuffer;
-#if 1 // prepared for reconnect
- bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
- if (retVal == false)
- {
- ARMARX_ERROR_S << "Timeout during waiting for new datagram";
- return sick_scan::ExitError;
- }
- else
- {
- // Look into receiving queue for new Datagrams
- //
- //
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
- }
- recvTimeStamp = datagramWithTimeStamp.timeStamp;
- dataBuffer = datagramWithTimeStamp.datagram;
-
- }
-#endif
- // dataBuffer = this->recvQueue.pop();
- long size = dataBuffer.size();
- memcpy(receiveBuffer, &(dataBuffer[0]), size);
- *actual_length = size;
-
- return sick_scan::ExitSuccess;
- }
-
-}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
deleted file mode 100644
index 6fe671d13f2974a6fba85835dfb09cf35282858a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Copyright (C) 2013, Freiburg University
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of Osnabrück University nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Created on: 15.11.2013
- *
- * Authors:
- * Christian Dornhege <c.dornhege@googlemail.com>
- */
-
-#ifndef SICK_TIM3XX_COMMON_TCP_H
-#define SICK_TIM3XX_COMMON_TCP_H
-
-#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <boost/asio.hpp>
-
-#undef NOMINMAX // to get rid off warning C4005: "NOMINMAX": Makro-Neudefinition
-
-#include <sick_scan/sick_scan_common.h>
-#include <sick_scan/sick_generic_parser.h>
-#include <sick_scan/template_queue.h>
-
-namespace armarx
-{
- /* class prepared for optimized time stamping */
-
- class DatagramWithTimeStamp
- {
- public:
- DatagramWithTimeStamp(IceUtil::Time timeStamp_, std::vector<unsigned char> datagram_)
- {
- timeStamp = timeStamp_;
- datagram = datagram_;
- }
-
- // private:
- IceUtil::Time timeStamp;
- std::vector<unsigned char> datagram;
- };
-
-
- class SickScanAdapter : public sick_scan::SickScanCommon
- {
- public:
- static void disconnectFunctionS(void* obj);
-
- SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
- char cola_dialect_id);
-
- virtual ~SickScanAdapter();
-
- int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- static void readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes);
-
- void readCallbackFunction(UINT8* buffer, UINT32& numOfBytes);
-
- void setReplyMode(int _mode);
-
- int getReplyMode();
-
- bool stopScanData();
-
- int numberOfDatagramInInputFifo();
-
- SopasEventMessage findFrameInReceiveBuffer();
-
- void processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame);
-
- // Queue<std::vector<unsigned char> > recvQueue;
- Queue<DatagramWithTimeStamp> recvQueue;
- UINT32 m_alreadyReceivedBytes;
- UINT32 m_lastPacketSize;
- UINT8 m_packetBuffer[480000];
- /**
- * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
- * hereingekommen sind.
- */
-
- protected:
- void disconnectFunction();
-
- void readCallbackFunctionOld(UINT8* buffer, UINT32& numOfBytes);
-
- virtual int init_device();
-
- virtual int close_device();
-
- /// Send a SOPAS command to the device and print out the response to the console.
- virtual int sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen);
-
- /// Read a datagram from the device.
- /**
- * \param [out] recvTimeStamp timestamp of received datagram
- * \param [in] receiveBuffer data buffer to fill
- * \param [in] bufferSize max data size to write to buffer (result should be 0 terminated)
- * \param [out] actual_length the actual amount of data written
- * \param [in] isBinaryProtocol true=binary False=ASCII
- */
- virtual int get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
-
- int getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer, int* actual_length, int* numberOfRemainingFifoEntries);
-
- // Helpers for boost asio
- int readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read = 0,
- bool* exception_occured = 0, bool isBinary = false);
-
- void handleRead(boost::system::error_code error, size_t bytes_transfered);
-
- void checkDeadline();
-
- private:
-
- // Response buffer
- UINT32 m_numberOfBytesInResponseBuffer; ///< Number of bytes in buffer
- UINT8 m_responseBuffer[1024]; ///< Receive buffer for everything except scan data and eval case data.
- Mutex m_receiveDataMutex; ///< Access mutex for buffer
-
- // Receive buffer
- UINT32 m_numberOfBytesInReceiveBuffer; ///< Number of bytes in buffer
- UINT8 m_receiveBuffer[480000]; ///< Low-Level receive buffer for all data
-
- bool m_beVerbose;
- bool m_emulSensor;
-
- boost::asio::io_service io_service_;
- boost::asio::ip::tcp::socket socket_;
- boost::asio::deadline_timer deadline_;
- boost::asio::streambuf input_buffer_;
- boost::system::error_code ec_;
- size_t bytes_transfered_;
-
- std::string hostname_;
- std::string port_;
- int timelimit_;
- int m_replyMode;
- };
-
-}
-#endif /* SICK_TIM3XX_COMMON_TCP_H */
-
diff --git a/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
deleted file mode 100644
index 0e95852596696c86a098f188f17370f808251b09..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore SickLaserUnit)
-
-armarx_add_test(SickLaserUnitTest SickLaserUnitTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp b/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp
deleted file mode 100644
index d6a69c168b0f9018569070829af43a8bd3094563..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * 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::SickLaserUnit
- * @author Johann Mantel ( j-mantel at gmx dot net )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::SickLaserUnit
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include "../SickLaserUnit.h"
-
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
-{
- armarx::SickLaserUnit instance;
-
- BOOST_CHECK_EQUAL(true, true);
-}