Merge branch 'master' into aron/image

# Conflicts:
#	source/RobotAPI/libraries/core/FramedPose.cpp
#	source/RobotAPI/libraries/core/LinkedPose.h
#	source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
#	source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
#	source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
#	source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
#	source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h

scenarios/RobotSkillsMemory/config/SkillsMemory.cfg

@@ -151,22 +151,6 @@
 # ArmarX.SkillsMemory.ObjectName = ""
 
 
-# ArmarX.SkillsMemory.StatechartCoreSegmentName:  Name of the core segment for statecharts.
-#  Attributes:
-#  - Default:            Statechart
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.SkillsMemory.StatechartCoreSegmentName = Statechart
-
-
-# ArmarX.SkillsMemory.TransitionsProviderSegmentName:  Name of the provider segment for statechart transitions.
-#  Attributes:
-#  - Default:            Transitions
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.SkillsMemory.TransitionsProviderSegmentName = Transitions
-
-
 # ArmarX.SkillsMemory.mem.MemoryName:  Name of this memory server.
 #  Attributes:
 #  - Default:            Skills

scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg

@@ -143,11 +143,19 @@
 # ArmarX.SickLaserUnit.ObjectName = ""
 
 
-# ArmarX.SickLaserUnit.angleOffset:  offset to the scanning angle
+# ArmarX.SickLaserUnit.TopicName:  Name of the laserscanner topic to report to.
 #  Attributes:
-#  - Default:            0
+#  - 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
 
 
@@ -156,7 +164,45 @@ ArmarX.SickLaserUnit.angleOffset = 0.0
 #  - Default:            LaserScannerFront,192.168.8.133,2112
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112
+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
@@ -196,7 +242,7 @@ ArmarX.SickLaserUnit.protocol = ASCII
 #  - Default:            10
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.SickLaserUnit.rangeMax = 10.00
+# ArmarX.SickLaserUnit.rangeMax = 10
 
 
 # ArmarX.SickLaserUnit.rangeMin:  minimum Range of the Scanner
@@ -204,20 +250,21 @@ ArmarX.SickLaserUnit.rangeMax = 10.00
 #  - Default:            0
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.SickLaserUnit.rangeMin = 0.01
+# ArmarX.SickLaserUnit.rangeMin = 0
 
 
 # ArmarX.SickLaserUnit.scannerType:  Name of the LaserScanner
 #  Attributes:
+#  - Default:            sick_tim_5xx
 #  - Case sensitivity:   yes
-#  - Required:           yes
-ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
+#  - Required:           no
+# ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
 
 
-# ArmarX.SickLaserUnit.timeIncrement:  timeIncrement??
+# ArmarX.SickLaserUnit.timeIncrement:  No Description
 #  Attributes:
-#  - Default:            0.10000000000000001
-#  - Case sensitivity:   yes
+#  - Default:            0.1
+#  - Case sensitivity:   no
 #  - Required:           no
 ArmarX.SickLaserUnit.timeIncrement = 0.1
 
@@ -230,22 +277,6 @@ ArmarX.SickLaserUnit.timeIncrement = 0.1
 # ArmarX.SickLaserUnit.timelimit = 5
 
 
-# ArmarX.SickLaserUnit.topicName:  Name of the topic
-#  Attributes:
-#  - Default:            SICKLaserScanner
-#  - Case sensitivity:   yes
-#  - Required:           no
-ArmarX.SickLaserUnit.topicName = "myLaserTopic1"
-
-
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit:  Name of the `LaserScannerUnit` topic to subscribe to.
-#  Attributes:
-#  - Default:            SICKLaserScanner
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.SickLaserUnit.tpc.sub.LaserScannerUnit = SICKLaserScanner
-
-
 # ArmarX.SickLaserUnit.updatePeriod:  No Description
 #  Attributes:
 #  - Default:            1

source/RobotAPI/applications/RobotControlUI/CMakeLists.txt

 
 armarx_component_set_name(RobotControlUI)
 
-set(COMPONENT_LIBS RobotAPIOperations)
+set(COMPONENT_LIBS RobotAPIOperations Simox::SimoxUtility)
 
 set(SOURCES main.cpp
     RobotControlUI.cpp

source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp

@@ -39,6 +39,9 @@ using namespace Ice;
 // ArmarX
 #include <RobotAPI/interface/core/PoseBase.h>
 #include <RobotAPI/libraries/core/Pose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
 #include <opencv2/core/core_c.h>
 
 using namespace Eigen;

source/RobotAPI/components/EarlyVisionGraph/CMakeLists.txt

@@ -3,6 +3,10 @@ armarx_component_set_name("EarlyVisionGraph")
 find_package(Eigen3 QUIET)
 armarx_build_if(Eigen3_FOUND "Eigen3 not available")
 
+set(COMPONENT_LIBS
+    Eigen3::Eigen
+)
+
 set(SOURCES
 ./MathTools.cpp
 ./SphericalGraph.cpp
@@ -26,6 +30,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 ")
 
 armarx_add_component("${SOURCES}" "${HEADERS}")
 if(Eigen3_FOUND)
-    target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR})
+    target_include_directories(EarlyVisionGraph SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
 endif()
 

source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h

@@ -24,6 +24,7 @@
 #include "../EigenForwardDeclarations.h"
 
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
 #include <ArmarXCore/core/util/StringHelpers.h>
 #include <ArmarXCore/observers/variant/TimedVariant.h>
 #include <ArmarXCore/interface/observers/VariantBase.h>

source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp

@@ -25,10 +25,6 @@
 
 // Include headers you only need in function definitions in the .cpp.
 
-// #include <Eigen/Core>
-
-// #include <SimoxUtility/color/Color.h>
-
 namespace armarx
 {
 
@@ -53,12 +49,12 @@ namespace armarx
                 case RunState::scannerRun:
                     if (result == sick_scan::ExitSuccess) // OK -> loop again
                     {
+                        scanData.clear();
                         result = scanner->loopOnce(scanData, scanTime, scanInfo, false);
                         if (scanTopic)
                         {
                             TimestampVariantPtr scanT(new TimestampVariant(scanTime));
-                            scanTopic->reportSensorValues(ip, scannerName, scanData, scanT);
-                            scanData.clear();
+                            scanTopic->reportSensorValues(scannerName, scannerName, scanData, scanT);
                             //trigger heartbeat
                             scannerHeartbeat->heartbeat(scannerName);
                         }
@@ -85,12 +81,12 @@ namespace armarx
 
     void SickLaserScanDevice::initScanner()
     {
-        ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
-                      << "] [Port: " << this->port << "]";
+        ARMARX_INFO_S << "Start initialising scanner " << scannerName
+                      << " [Ip: " << this->ip << "] [Port: " << this->port << "]";
         // attempt to connect/reconnect
         if (this->scanner)
         {
-            ARMARX_WARNING_S << "Scanner already initized.";
+            ARMARX_WARNING_S << "Scanner already initialized - reinit.";
             this->scanner.reset(); // disconnect scanner
         }
         this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
@@ -108,7 +104,6 @@ namespace armarx
         }
         else
         {
-            ARMARX_INFO_S << "Reinitializing scanner.";
             this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
         }
     }
@@ -131,12 +126,13 @@ namespace armarx
         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.heartbeatWarnMS, "heartbeatWarnMS", "maximum cycle time before heartbeat Warning");
+        def->optional(properties.heartbeatErrorMS, "heartbeatErrorMS", "maximum cycle time before heartbeat Error");
         return def;
     }
 
     SickLaserUnit::SickLaserUnit()
     {
-        //heartbeat = addPlugin<plugins::HeartbeatComponentPlugin>("SickLaserUnit");
         addPlugin(heartbeat);
         ARMARX_CHECK_NOT_NULL(heartbeat);
     }
@@ -145,6 +141,7 @@ namespace armarx
     {
         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.)
@@ -163,18 +160,8 @@ namespace armarx
                 continue;
             }
             SickLaserScanDevice& device = scanDevices.emplace_back();
-            device.scanTopic = topic;
             device.scannerName = deviceInfo[0];
-            if (deviceInfo[1] != "")
-            {
-                device.ip = deviceInfo[1];
-            }
-            else
-            {
-                ARMARX_FATAL << "TCP is not switched on. Probably hostname or port not set.";
-                return;
-
-            }
+            device.ip = deviceInfo[1];
             device.port = deviceInfo[2];
             device.timelimit = properties.timelimit;
             //scanInfo
@@ -193,8 +180,10 @@ namespace armarx
                 ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
                 return;
             }
+            armarx::RobotHealthHeartbeatArgs heartbeatArgs =
+                armarx::RobotHealthHeartbeatArgs(properties.heartbeatWarnMS, properties.heartbeatErrorMS, "No LaserScan data available");
             device.scannerHeartbeat = heartbeat;
-            device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, armarx::RobotHealthHeartbeatArgs(100, 200, "No updates available"));
+            device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, heartbeatArgs);
         }
     }
 

source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h

@@ -27,7 +27,6 @@
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
-//#include <RobotAPI/interface/components/RobotHealthInterface.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
 
 #include <vector>
@@ -55,7 +54,7 @@ namespace armarx
 
     struct SickLaserScanDevice
     {
-        std::string scannerName = "LaserScannerFront";
+        std::string scannerName;
         //communication parameters
         std::string ip;
         std::string port;
@@ -125,18 +124,20 @@ namespace armarx
         /// Properties shown in the Scenario GUI.
         struct Properties
         {
-            std::string topicName = "SICKLaserScanner";
+            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 = 10.0;
+            int heartbeatWarnMS = 500;
+            int heartbeatErrorMS = 800;
         };
         Properties properties;
         std::vector<SickLaserScanDevice> scanDevices;
         LaserScannerUnitListenerPrx topic;
-        plugins::HeartbeatComponentPlugin* heartbeat;
+        plugins::HeartbeatComponentPlugin* heartbeat = NULL;
 
     };
 } // namespace armarx

source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp

@@ -88,15 +88,15 @@
 //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
-}
+//static int getDiagnosticErrorCode()
+//{
+//#ifdef _MSC_VER
+//#undef ERROR
+//    return (2);
+//#else
+//    return (diagnostic_msgs::DiagnosticStatus::ERROR);
+//#endif
+//}
 
 namespace armarx
 {
@@ -154,9 +154,8 @@ namespace armarx
         int actual_length = 0;
         int packetsInLoop = 0;
 
-        ros::Time recvTimeStamp = ros::Time::now();  // timestamp incoming package, will be overwritten by get_datagram
-        //use ASCII always
-        int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
+        //always use ASCII
+        int result = getDatagramIceTime(scanTime, receiveBuffer, &actual_length, &packetsInLoop);
         //ros::Duration dur = recvTimeStampPush - recvTimeStamp;
         if (result != 0)
         {
@@ -168,19 +167,13 @@ namespace armarx
             return sick_scan::ExitSuccess;
         } // return success to continue looping
 
-        //convert ros::Time recvTimeStamp to IceUtil
-        ros::Time correctedStamp = recvTimeStamp + ros::Duration(config_.time_offset);
-        uint64_t recvMsec = correctedStamp.toNSec() / 1000;
-        scanTime = IceUtil::Time::microSeconds(recvMsec);
-
         //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
         char* buffer_pos = (char*) receiveBuffer;
-        char* dstart, *dend;
+        char* dstart = NULL;
+        char* dend = NULL;
         bool dataToProcess = true;
         std::vector<float> vang_vec;
         vang_vec.clear();
-        dstart = NULL;
-        dend = NULL;
 
         while (dataToProcess)
         {
@@ -309,7 +302,7 @@ namespace armarx
             }
         }
 
-        //ros::Time start_time = ros::Time::now(); // will be adjusted in the end
+        //IceUtil::Time start_time = IceUtil::Time::now(); // will be adjusted in the end
 
         // <STX> (\x02)
         // 0: Type of command (SN)
@@ -668,7 +661,7 @@ namespace armarx
     * hereingekommen sind.
     */
 
-    void SickScanAdapter::processFrame(ros::Time timeStamp, SopasEventMessage& frame)
+    void SickScanAdapter::processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame)
     {
 
         if (getProtocolType() == CoLa_A)
@@ -697,11 +690,8 @@ namespace armarx
 
     void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
     {
-        ros::Time rcvTimeStamp = ros::Time::now(); // stamp received datagram
+        IceUtil::Time rcvTimeStamp = IceUtil::Time::now(); // stamp received datagram
         bool beVerboseHere = false;
-        printInfoMessage(
-            "SickScanCommonNw::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.",
-            beVerboseHere);
 
         ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
         UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
@@ -949,10 +939,14 @@ namespace armarx
         return sick_scan::ExitSuccess;
     }
 
-
-    int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize,
-                                      int* actual_length,
+    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)
         {
@@ -988,61 +982,6 @@ namespace armarx
         memcpy(receiveBuffer, &(dataBuffer[0]), size);
         *actual_length = size;
 
-#if 0
-        static int cnt = 0;
-        char szFileName[255];
-        sprintf(szFileName, "/tmp/dg%06d.bin", cnt++);
-
-        FILE* fout;
-
-        fout = fopen(szFileName, "wb");
-        if (fout != NULL)
-        {
-            fwrite(receiveBuffer, size, 1, fout);
-            fclose(fout);
-        }
-#endif
-        return sick_scan::ExitSuccess;
-
-        if (!socket_.is_open())
-        {
-            ROS_ERROR("get_datagram: socket not open");
-            diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: socket not open.");
-            return sick_scan::ExitError;
-        }
-
-        /*
-         * Write a SOPAS variable read request to the device.
-         */
-        std::vector<unsigned char> reply;
-
-        // Wait at most 5000ms for a new scan
-        size_t timeout = 30000;
-        bool exception_occured = false;
-
-        char* buffer = reinterpret_cast<char*>(receiveBuffer);
-
-        if (readWithTimeout(timeout, buffer, bufferSize, actual_length, &exception_occured, isBinaryProtocol) !=
-            sick_scan::ExitSuccess)
-        {
-            ROS_ERROR_THROTTLE(1.0, "get_datagram: no data available for read after %zu ms", timeout);
-            diagnostics_.broadcast(getDiagnosticErrorCode(), "get_datagram: no data available for read after timeout.");
-
-            // Attempt to reconnect when the connection was terminated
-            if (!socket_.is_open())
-            {
-#ifdef _MSC_VER
-                Sleep(1000);
-#else
-                sleep(1);
-#endif
-                ARMARX_INFO << "Failure - attempting to reconnect";
-                return init();
-            }
-
-            return exception_occured ? sick_scan::ExitError : sick_scan::ExitSuccess;    // keep on trying
-        }
-
         return sick_scan::ExitSuccess;
     }
 

source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h

@@ -57,14 +57,14 @@ namespace armarx
     class DatagramWithTimeStamp
     {
     public:
-        DatagramWithTimeStamp(ros::Time timeStamp_, std::vector<unsigned char> datagram_)
+        DatagramWithTimeStamp(IceUtil::Time timeStamp_, std::vector<unsigned char> datagram_)
         {
             timeStamp = timeStamp_;
             datagram = datagram_;
         }
 
         // private:
-        ros::Time timeStamp;
+        IceUtil::Time timeStamp;
         std::vector<unsigned char> datagram;
     };
 
@@ -97,7 +97,7 @@ namespace armarx
 
         SopasEventMessage findFrameInReceiveBuffer();
 
-        void processFrame(ros::Time timeStamp, SopasEventMessage& frame);
+        void processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame);
 
         // Queue<std::vector<unsigned char> > recvQueue;
         Queue<DatagramWithTimeStamp> recvQueue;
@@ -129,9 +129,10 @@ namespace armarx
          * \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);
+        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,
@@ -143,7 +144,6 @@ namespace armarx
 
     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.

source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp

@@ -21,8 +21,10 @@
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
+
 #include "WeissHapticSensor.h"
 #include "TransmissionException.h"
+
 #include <boost/regex.hpp>
 #include <boost/format.hpp>
 

source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h

@@ -23,17 +23,19 @@
  */
 #pragma once
 
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
 #include "SerialInterface.h"
 #include "TactileSensor.h"
+#include "TextWriter.h"
+
 #include <RobotAPI/interface/units/HapticUnit.h>
+#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
+
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
 #include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
-//#include <ArmarXCore/util/variants/eigen3/Eigen3LibRegistry.h>
-#include "TextWriter.h"
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
 #include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <ArmarXCore/core/logging/Logging.h>
 
 #include <mutex>
 

source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.ipp

@@ -6,8 +6,7 @@
 #include <SimoxUtility/meta/enum/adapt_enum.h>
 
 #include <ArmarXCore/util/CPPUtility/trace.h>
-
-#include "box_to_grasp_candidates.ipp"
+#include <ArmarXCore/core/logging/Logging.h>
 
 
 namespace simox::meta

source/RobotAPI/libraries/core/LinkedPose.h

@@ -42,6 +42,7 @@ namespace armarx::VariantType
     // variant types
     const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase");
     const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
+
     void suppressWarningUnusedVariableForLinkedPoseAndDirection();
 }
 

source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp

@@ -25,6 +25,7 @@
 
 #include <ArmarXCore/core/util/StringHelpers.h>
 
+
 namespace armarx
 {
 

source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h

@@ -23,13 +23,12 @@
  */
 #pragma once
 
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ImportExport.h>
-#include <ArmarXCore/observers/ConditionCheck.h>
-
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <RobotAPI/libraries/core/LinkedPose.h>
 
+#include <ArmarXCore/observers/ConditionCheck.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
 
 namespace armarx
 {

source/RobotAPI/libraries/core/math/StatUtils.h

@@ -22,7 +22,9 @@
 
 #pragma once
 
-#include <math.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+
+#include <cmath>
 #include <vector>
 
 namespace armarx::math

source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h

@@ -22,9 +22,12 @@
 
 #pragma once
 
-#include <ArmarXCore/observers/filters/DatafieldFilter.h>
 #include <RobotAPI/interface/observers/ObserverFilters.h>
+
+#include <ArmarXCore/observers/filters/DatafieldFilter.h>
 #include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
 #include <algorithm>
 
 namespace armarx::filters

source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp

@@ -21,6 +21,7 @@
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
+
 #include "MedianDerivativeFilterV3.h"
 
 #include <ArmarXCore/core/logging/Logging.h>

source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp

@@ -21,6 +21,7 @@
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
+
 #include "PoseMedianOffsetFilter.h"
 
 #include <ArmarXCore/core/logging/Logging.h>