diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 950a5a53fe6962a8f24298b7111fde09aa8d6d89..e652a1057af76e89e465615ea152beaab0829f3d 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -7,5 +7,6 @@ add_subdirectory(ObjectMemory)
 add_subdirectory(ReasoningMemory)
 add_subdirectory(RobotStateMemory)
 add_subdirectory(SkillsMemory)
+add_subdirectory(LaserScansMemory)
 #add_subdirectory(SubjectMemory)
 add_subdirectory(SystemStateMemory)
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a94f44f2160ec2ee4ab67618a8552ffdbdf8ed28
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("LaserScansMemory")
+
+
+set(COMPONENT_LIBS
+    ArmarXCore ArmarXCoreInterfaces  # for DebugObserverInterface
+    ArmarXCoreComponentPlugins
+    ArmarXGuiComponentPlugins
+    RobotAPICore RobotAPIInterfaces armem_server
+    RobotAPIComponentPlugins  # for ArViz and other plugins
+    armem_robot_state
+    armem_robot
+
+    armem_laser_scans
+
+    ${IVT_LIBRARIES}
+)
+
+set(SOURCES
+    LaserScansMemory.cpp
+)
+set(HEADERS
+    LaserScansMemory.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE armarx::armem::server::laser_scans)
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63d5bdbd644a75960a9c3824d870faf69f1eb476
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
@@ -0,0 +1,105 @@
+/**
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "LaserScansMemory.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "RobotAPI/libraries/armem/server/MemoryToIceAdapter.h"
+#include "RobotAPI/libraries/armem_laser_scans/constants.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+
+
+namespace armarx::armem::server::laser_scans
+{
+    LaserScansMemory::LaserScansMemory()
+    {
+        addPlugin(virtualRobotReaderPlugin);
+        addPlugin(debugObserver);
+    }
+
+    armarx::PropertyDefinitionsPtr
+    LaserScansMemory::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr defs =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        workingMemory().name() = "LaserScans";
+
+        {
+            const std::string prefix = "";
+            commonVisu.defineProperties(defs, prefix + "visu.");
+        }
+
+        return defs;
+    }
+
+
+    std::string
+    LaserScansMemory::getDefaultName() const
+    {
+        return "LaserScansMemory";
+    }
+
+
+    void
+    LaserScansMemory::onInitComponent()
+    {
+        const auto& coreSegment = workingMemory().addCoreSegment(
+            "LaserScans", armarx::armem::laser_scans::arondto::LaserScanStamped::ToAronType());
+
+        commonVisu.init(&coreSegment, &virtualRobotReaderPlugin->get());
+    }
+
+
+    void
+    LaserScansMemory::onConnectComponent()
+    {
+        commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
+    }
+
+
+    void
+    LaserScansMemory::onDisconnectComponent()
+    {
+    }
+
+
+    void
+    LaserScansMemory::onExitComponent()
+    {
+    }
+
+
+    armem::data::AddSegmentsResult
+    LaserScansMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+    {
+        // Allowing adding core segments.
+        armem::data::AddSegmentsResult result =
+            ReadWritePluginUser::addSegments(input, addCoreSegmentOnUsage);
+        return result;
+    }
+
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b1275985b57f5d3bf8ad6bcfd939c186f7462ba
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h
@@ -0,0 +1,89 @@
+/**
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_laser_scans/server/Visu.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+
+namespace armarx::armem::server::laser_scans
+{
+    /**
+ * @defgroup Component-GeneralPurposeMemory GeneralPurposeMemory
+ * @ingroup RobotAPI-Components
+ * A description of the component GeneralPurposeMemory.
+ *SpecializedCoreSegment
+ * @class GeneralPurposeMemory
+ * @ingroup Component-GeneralPurposeMemory
+ * @brief Brief description of class GeneralPurposeMemory.
+ *
+ * Detailed description of class GeneralPurposeMemory.
+ */
+    class LaserScansMemory :
+        virtual public armarx::Component,
+        virtual public armem::server::ReadWritePluginUser,
+        virtual public armarx::ArVizComponentPluginUser
+    {
+    public:
+        LaserScansMemory();
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+        armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input,
+                                                   const Ice::Current&) override;
+
+    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:
+        bool addCoreSegmentOnUsage = true;
+
+        armem::client::plugins::ReaderWriterPlugin<armarx::armem::robot_state::VirtualRobotReader>*
+            virtualRobotReaderPlugin = nullptr;
+
+        armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
+
+
+        Visu commonVisu;
+    };
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index af02b7d0e859068bf508d27316ce719517c1de17..2c723f426a268f2a9e01aee60c3a39acee225bd6 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -33,6 +33,8 @@ armarx_add_component(
 	RobotAPICore
 	## RobotAPIInterfaces
 	RobotAPIComponentPlugins  # For ArViz and other plugins.
+	armem_laser_scans
+	LaserScansMemory # component
 
 	# This project
 	## ${PROJECT_NAME}Interfaces  # For ice interfaces from this package.
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 8fdadffeb177990068d890b26551b88d5712cc6f..23bb88f2d0a433961d8f88e41bf4d496698571c3 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -21,14 +21,18 @@
  */
 
 #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()
+    void
+    SickLaserScanDevice::run()
     {
         while (!task->isStopped())
         {
@@ -42,7 +46,8 @@ namespace armarx
                     }
                     else
                     {
-                        ARMARX_WARNING << "Maximum number of reinitializations reached - going to idle state";
+                        ARMARX_WARNING
+                            << "Maximum number of reinitializations reached - going to idle state";
                         runState = RunState::scannerFinalize;
                     }
                     break;
@@ -51,16 +56,27 @@ namespace armarx
                     {
                         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);
+                            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;
+                            ARMARX_WARNING << "No scan topic available: IP: " << ip
+                                           << ", Port: " << port;
                         }
                     }
                     else
@@ -79,27 +95,32 @@ namespace armarx
         }
     }
 
-    void SickLaserScanDevice::initScanner()
+    void
+    SickLaserScanDevice::initScanner()
     {
-        ARMARX_INFO_S << "Start initialising scanner " << scannerName
-                      << " [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 initialized - reinit.";
+            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->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 successfully initialized.";
+            ARMARX_INFO_S << "Scanner `" << ip << "` successfully initialized.";
             this->runState = RunState::scannerRun; // after initialising switch to run state
         }
         else
@@ -109,7 +130,8 @@ namespace armarx
     }
 
 
-    armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
+    armarx::PropertyDefinitionsPtr
+    SickLaserUnit::createPropertyDefinitions()
     {
         armarx::PropertyDefinitionsPtr def =
             new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -119,15 +141,20 @@ namespace armarx
         // 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.");
+        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.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.heartbeatWarnMS, "heartbeatWarnMS", "maximum cycle time before heartbeat Warning");
-        def->optional(properties.heartbeatErrorMS, "heartbeatErrorMS", "maximum cycle time before heartbeat Error");
+
+        def->optional(properties.robotName, "robotName", "");
         return def;
     }
 
@@ -135,9 +162,23 @@ namespace armarx
     {
         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()
+    void
+    SickLaserUnit::onInitComponent()
     {
         ARMARX_INFO << "On init";
         // Topics and properties defined above are automagically registered.
@@ -167,10 +208,13 @@ namespace armarx
             //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 =
+                    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);
@@ -180,18 +224,19 @@ 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, heartbeatArgs);
+            device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName,
+                                                               "No LaserScan data available");
         }
     }
 
-    void SickLaserUnit::onConnectComponent()
+    void
+    SickLaserUnit::onConnectComponent()
     {
         for (SickLaserScanDevice& device : scanDevices)
         {
             device.scanTopic = topic;
+            device.laserScanWriter = &laserScanWriterPlugin->get();
             //start the laser scanner
             if (device.task)
             {
@@ -200,7 +245,8 @@ namespace armarx
                 device.task = nullptr;
             }
             device.runState = RunState::scannerInit;
-            device.task = new RunningTask<SickLaserScanDevice>(&device, &SickLaserScanDevice::run, "SickLaserScanUpdate_" + device.ip);
+            device.task = new RunningTask<SickLaserScanDevice>(
+                &device, &SickLaserScanDevice::run, "SickLaserScanUpdate_" + device.ip);
             device.task->start();
         }
         // Do things after connecting to topics and components.
@@ -217,7 +263,8 @@ namespace armarx
         */
     }
 
-    void SickLaserUnit::onDisconnectComponent()
+    void
+    SickLaserUnit::onDisconnectComponent()
     {
         ARMARX_INFO_S << "Disconnecting LaserScanner.";
         for (SickLaserScanDevice& device : scanDevices)
@@ -230,11 +277,13 @@ namespace armarx
         }
     }
 
-    void SickLaserUnit::onExitComponent()
+    void
+    SickLaserUnit::onExitComponent()
     {
     }
 
-    std::string SickLaserUnit::getDefaultName() const
+    std::string
+    SickLaserUnit::getDefaultName() const
     {
         return "SickLaserUnit";
     }
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 8fa16e4bf6961f13e1cd8acdf64b9acea8e40332..1fed1aab9d05370280535c4314d9a4104ce25fcb 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -26,8 +26,10 @@
 
 #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>
 
@@ -69,6 +71,10 @@ namespace armarx
         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;
@@ -128,16 +134,21 @@ namespace armarx
             //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;
+            double rangeMax = 12.0;
+
+            std::string robotName = "Armar6";
         };
         Properties properties;
         std::vector<SickLaserScanDevice> scanDevices;
         LaserScannerUnitListenerPrx topic;
-        plugins::HeartbeatComponentPlugin* heartbeat = NULL;
+        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
index 5cabc89ddbb4843acc00a24c42a35b03d87f99b8..3d20aad67f1a121c641bfa4f82021c9d2a59d74b 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -47,6 +47,8 @@
 *
 */
 
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <VirtualRobot/MathTools.h>
 #ifdef _MSC_VER
 #pragma warning(disable: 4996)
 #pragma warning(disable: 4267)
@@ -78,10 +80,10 @@
 #include <boost/lexical_cast.hpp>
 #include <vector>
 
-#ifndef rad2deg
-#define rad2deg(x) ((x) / M_PI * 180.0)
-#endif
-#define deg2rad_const (0.017453292519943295769236907684886f)
+// #ifndef rad2deg
+// #define rad2deg(x) ((x) / M_PI * 180.0)
+// #endif
+// #define deg2rad_const (0.017453292519943295769236907684886f)
 
 
 //std::vector<unsigned char> exampleData(65536);
@@ -406,7 +408,7 @@ namespace armarx
                     {
                         unsigned short iRange;
                         sscanf(fields[offset + i], "%hx", &iRange);
-                        float range = iRange / 1000.0;
+                        float range = iRange; // / 1000.0;
                         distVal.push_back(range);
                     }
                     else
@@ -429,11 +431,23 @@ namespace armarx
             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;
+            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);
@@ -685,7 +699,7 @@ namespace armarx
                 frame.getRawData() +
                 frame.size()));
         // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
-        recvQueue.commit(dataGramWidthTimeStamp);
+        recvQueue.push(dataGramWidthTimeStamp);
     }
 
     void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 735586f822c6c0f9ef697c15afd2a625862e927f..ecdd9f825e6442376cd19cd0c3a61a2ac590f51b 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -31,10 +31,10 @@ add_subdirectory(armem_robot)
 add_subdirectory(armem_robot_state)
 add_subdirectory(armem_skills)
 add_subdirectory(armem_system_state)
+add_subdirectory(armem_laser_scans)
 add_subdirectory(armem_vision)
 
 add_subdirectory(skills)
 
 add_subdirectory(RobotUnitDataStreamingReceiver)
 add_subdirectory(GraspingUtility)
-
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index fc83132850cfb652359c0ef7cca81d89e3ff3aac..41b48bdff28d2cdbd2e601ab7e9d6dadcdbc0e2b 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -13,6 +13,13 @@ namespace armarx::plugins
         channelHeartbeatConfig.emplace(channel, args);
     }
 
+    void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, const std::string& message)
+    {
+        auto args = heartbeatArgs;
+        args.message = message;
+        configureHeartbeatChannel(channel, args);
+    }
+
     void HeartbeatComponentPlugin::heartbeat()
     {
 
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
index 80cf546726085b6ca03406a7dc1abc91e647c11c..bbefa079c1d54e71444d9f0e90120f4db0250297 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -33,6 +33,14 @@ namespace armarx::plugins
     public:
         using ComponentPlugin::ComponentPlugin;
 
+        /**
+         * @brief Configures a heartbeat subchannel.
+         *
+         * @param channel Identifier of the heartbeat channel
+         * @param args Configuration of this channel's heartbeat properties
+         */
+        void configureHeartbeatChannel(const std::string& channel, const std::string& message);
+
         /**
          * @brief Configures a heartbeat subchannel.
          *
@@ -66,7 +74,7 @@ namespace armarx::plugins
 
     private:
         //! heartbeat topic name (outgoing)
-        std::string topicName{"DebugObserver"};
+        std::string topicName{"RobotHealthTopic"};
 
         //! name of this component used as identifier for heartbeats
         std::string componentName;
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index b7d6343d447fd95a6031b2ab5b5c88201a540069..b83206f6fb670587dc418fdd0207382390b83543 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -22,6 +22,7 @@
 
 #include <Ice/ObjectAdapter.h>
 
+#include "ArmarXCore/core/logging/Logging.h"
 #include <ArmarXCore/core/ArmarXManager.h>
 
 #include "RobotUnitDataStreamingReceiver.h"
@@ -59,8 +60,8 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
             static_assert(sizeof(std::uint64_t) == sizeof(msgSequenceNbr));
             const auto seq = static_cast<std::uint64_t>(msgSequenceNbr);
             std::lock_guard g{_data_mutex};
-            ARMARX_INFO << deactivateSpam()
-                        << "received " << data.size() << " timesteps";
+            ARMARX_VERBOSE << deactivateSpam()
+                           << "received " << data.size() << " timesteps";
             _data[seq] = data;
         }
 
diff --git a/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1d452ddcb4d41e37379cf21364bd914e59e328c5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
@@ -0,0 +1,42 @@
+set(LIB_NAME armem_laser_scans)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+    LIBS
+        # ArmarX
+        ArmarXCore
+        # This package
+        RobotAPI::Core
+        RobotAPI::armem
+        aroncommon
+        armem_robot_state
+        armem_robot
+        # System / External
+        Eigen3::Eigen
+    HEADERS
+        aron_conversions.h
+        client/common/Reader.h
+        client/common/Writer.h
+        server/Visu.h
+        constants.h
+    SOURCES
+        aron_conversions.cpp
+        client/common/Reader.cpp
+        client/common/Writer.cpp
+        server/Visu.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+    TARGET_NAME
+        "${LIB_NAME}"
+    ARON_FILES
+        aron/LaserScan.xml
+)
+
+add_library(
+    RobotAPI::armem_laser_scans
+    ALIAS
+    armem_laser_scans
+)
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_laser_scans/aron/LaserScan.xml
similarity index 80%
rename from source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
rename to source/RobotAPI/libraries/armem_laser_scans/aron/LaserScan.xml
index 9ad92092e4f2fde9a14450e0bf36bacb8cc84326..dfaae40135956c8c342e8ebc043ce2a3775d10f6 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron/LaserScan.xml
@@ -8,7 +8,7 @@
 
     <GenerateTypes>
 
-        <Object name='armarx::armem::vision::arondto::LaserScannerInfo'>
+        <Object name='armarx::armem::laser_scans::arondto::LaserScannerInfo'>
             <ObjectChild key='device'>
                 <string />
             </ObjectChild>
@@ -26,7 +26,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name="armarx::armem::vision::arondto::SensorHeader">
+        <Object name="armarx::armem::laser_scans::arondto::SensorHeader">
             <ObjectChild key="agent">
                 <string/>
             </ObjectChild>
@@ -39,9 +39,9 @@
         </Object>
 
 
-        <Object name='armarx::armem::vision::arondto::LaserScanStamped'>
+        <Object name='armarx::armem::laser_scans::arondto::LaserScanStamped'>
             <ObjectChild key="header">
-                <armarx::armem::vision::arondto::SensorHeader />
+                <armarx::armem::laser_scans::arondto::SensorHeader />
             </ObjectChild>
 
             <!-- 
diff --git a/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aaa39472e70e3152feee0f306aca66b81d3f7343
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
@@ -0,0 +1,100 @@
+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <iterator>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/common/Converter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "types.h"
+
+
+namespace armarx::armem::laser_scans
+{
+
+    /************ fromAron ************/
+    SensorHeader
+    fromAron(const arondto::SensorHeader& aronSensorHeader)
+    {
+        return {.agent = aronSensorHeader.agent,
+                .frame = aronSensorHeader.frame,
+                .timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
+    }
+
+    void
+    fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
+    {
+        laserScan.header = fromAron(aronLaserScan.header);
+        // laserScan.data = fromAron(aronLaserScan.data);
+    }
+
+    void
+    fromAron(const arondto::LaserScanStamped& aronLaserScan,
+             LaserScan& laserScan,
+             std::int64_t& timestamp,
+             std::string& frame,
+             std::string& agentName)
+    {
+        const auto header = fromAron(aronLaserScan.header);
+
+        // laserScan = fromAron(aronLaserScan.data);
+
+        timestamp = header.timestamp.toMicroSecondsSinceEpoch();
+        frame = header.frame;
+        agentName = header.agent;
+    }
+
+    /************ toAron ************/
+
+    // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+    // {
+    //     aronLaserScan.scan = toAron(laserScan);
+    // }
+
+    int64_t
+    toAron(const armem::Time& timestamp)
+    {
+        return timestamp.toMicroSecondsSinceEpoch();
+    }
+
+    arondto::SensorHeader
+    toAron(const SensorHeader& sensorHeader)
+    {
+        arondto::SensorHeader aronSensorHeader;
+
+        aronSensorHeader.agent = sensorHeader.agent;
+        aronSensorHeader.frame = sensorHeader.frame;
+        aron::toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
+
+        return aronSensorHeader;
+    }
+
+    void
+    toAron(const LaserScanStamped& laserScanStamped,
+           arondto::LaserScanStamped& aronLaserScanStamped)
+    {
+        aronLaserScanStamped.header = toAron(laserScanStamped.header);
+        // toAron(laserScanStamped.data, aronLaserScanStamped.data);
+    }
+
+    void
+    toAron(const LaserScan& laserScan,
+           const armem::Time& timestamp,
+           const std::string& frame,
+           const std::string& agentName,
+           arondto::LaserScanStamped& aronLaserScanStamped)
+    {
+        const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
+
+        const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+
+        toAron(laserScanStamped, aronLaserScanStamped);
+    }
+
+
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.h b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..931cda8c82013b0d2ad9060bd39be4273e9fce51
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.h
@@ -0,0 +1,71 @@
+/*
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+namespace armarx::armem::laser_scans
+{
+
+    namespace arondto
+    {
+        struct LaserScanStamped;
+    } // namespace arondto
+
+    // struct LaserScan;
+    struct LaserScanStamped;
+
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+                  LaserScan& laserScan,
+                  std::int64_t& timestamp,
+                  std::string& frame,
+                  std::string& agentName);
+
+    template <typename T>
+    auto
+    fromAron(const aron::data::NDArrayPtr& navigator)
+    {
+        return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+    }
+
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+
+    void toAron(const LaserScan& laserScan,
+                const armem::Time& timestamp,
+                const std::string& frame,
+                const std::string& agentName,
+                arondto::LaserScanStamped& aronLaserScan);
+
+    inline aron::data::NDArrayPtr
+    toAron(const LaserScan& laserScan)
+    {
+        using aron::converter::AronVectorConverter;
+        return AronVectorConverter::ConvertFromVector(laserScan);
+    }
+
+
+} // namespace armarx::armem::laser_scans
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
similarity index 86%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index 396dd1bd8b4f40cfef062fa963e87c97f52f45b4..ec17f944c60747160a3306032fa89c13899d6b86 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -24,6 +24,7 @@
 #include <ArmarXCore/core/logging/Logging.h>
 
 // RobotAPI Interfaces
+#include "RobotAPI/libraries/armem_laser_scans/constants.h"
 #include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
 #include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
@@ -42,12 +43,12 @@
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/selectors.h>
 
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
 
 
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
 {
 
     Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
@@ -65,22 +66,17 @@ namespace armarx::armem::vision::laser_scans::client
 
         const std::string prefix = propertyPrefix;
 
-        def->optional(properties.coreSegmentName,
-                      prefix + "CoreSegment",
-                      "Name of the mapping memory core segment to use.");
-
-        def->optional(properties.memoryName, prefix + "MemoryName");
     }
 
     void Reader::connect()
     {
         // Wait for the memory to become available and add it as dependency.
         ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
-                         << properties.memoryName << "' ...";
+                         << constants::memoryName << "' ...";
         try
         {
-            memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
-            ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName << "'";
+            memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
+            ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName << "'";
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)
         {
@@ -94,14 +90,14 @@ namespace armarx::armem::vision::laser_scans::client
     {
         armarx::armem::client::query::Builder qb;
 
-        ARMARX_INFO << "Query for agent: " << query.agent
-                    << " memory name: " << properties.memoryName;
+        ARMARX_VERBOSE << "Query for agent: " << query.agent
+                    << " memory name: " << constants::memoryName;
 
         if (query.sensorList.empty()) // all sensors
         {
             // clang-format off
             qb
-            .coreSegments().withName(properties.memoryName)
+            .coreSegments().withName(constants::memoryName)
             .providerSegments().withName(query.agent)
             .entities().all()
             .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -111,7 +107,7 @@ namespace armarx::armem::vision::laser_scans::client
         {
             // clang-format off
             qb
-            .coreSegments().withName(properties.memoryName)
+            .coreSegments().withName(constants::memoryName)
             .providerSegments().withName(query.agent)
             .entities().withNames(query.sensorList)
             .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -144,7 +140,6 @@ namespace armarx::armem::vision::laser_scans::client
             ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
 
             laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
-            ARMARX_IMPORTANT << "4";
 
             return laserScanStamped;
         };
@@ -196,7 +191,7 @@ namespace armarx::armem::vision::laser_scans::client
 
         // now create result from memory
         const wm::ProviderSegment& providerSegment =
-            qResult.memory.getCoreSegment(properties.memoryName).getProviderSegment(query.agent);
+            qResult.memory.getCoreSegment(constants::memoryName).getProviderSegment(query.agent);
 
         const auto laserScans = asLaserScans(providerSegment);
         std::vector<std::string> sensors;
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
similarity index 93%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 349b65a5af3bea2fc6acb17324a42b87fbb3bf8d..70703521336f8ac0b1fdf9649df4a3777d95f431 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -27,11 +27,11 @@
 
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 
+#include "RobotAPI/libraries/armem_laser_scans/types.h"
 #include <RobotAPI/libraries/armem/client.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
 
 
 namespace armarx
@@ -39,7 +39,7 @@ namespace armarx
     class ManagedIceObject;
 }
 
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
 {
 
     struct TimeRange
@@ -109,8 +109,6 @@ namespace armarx::armem::vision::laser_scans::client
         // Properties
         struct Properties
         {
-            std::string memoryName      = "Vision";
-            std::string coreSegmentName = "LaserScans";
         } properties;
 
         const std::string propertyPrefix = "mem.vision.laser_scans.";
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
similarity index 54%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
index 0cfe7fb95f4abdf1d5065a6d0f245e516644bc8f..08f456b165d08e52c9c3ce0aed649aa96ec8f863 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
@@ -1,15 +1,19 @@
 #include "Writer.h"
 
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include "RobotAPI/libraries/armem_laser_scans/constants.h"
 #include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
 
 
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
 {
 
-    Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem)
-        : memoryNameSystem(memoryNameSystem) {}
+    Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+        memoryNameSystem(memoryNameSystem)
+    {
+    }
     Writer::~Writer() = default;
 
 
@@ -20,22 +24,20 @@ namespace armarx::armem::vision::laser_scans::client
 
         const std::string prefix = propertyPrefix;
 
-        def->optional(properties.coreSegmentName,
-                      prefix + "CoreSegment",
-                      "Name of the mapping memory core segment to use.");
-
-        def->optional(properties.memoryName, prefix + "MemoryName");
     }
 
-    void Writer::connect()
+    void
+    Writer::connect()
     {
         // Wait for the memory to become available and add it as dependency.
-        ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
-                         << properties.memoryName << "' ...";
+        ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
+                         << "' ...";
         try
         {
-            memoryWriter = memoryNameSystem.useWriter(MemoryID().withMemoryName(properties.memoryName));
-            ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName << "'";
+            memoryWriter =
+                memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
+            ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
+                             << "'";
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)
         {
@@ -43,19 +45,20 @@ namespace armarx::armem::vision::laser_scans::client
             return;
         }
 
-        ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
-                         << properties.memoryName;
+        ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
     }
 
-    bool Writer::storeSensorData(const LaserScan& laserScan,
-                                 const std::string& frame,
-                                 const std::string& agentName,
-                                 const std::int64_t& timestamp)
+    bool
+    Writer::storeSensorData(const LaserScan& laserScan,
+                            const std::string& frame,
+                            const std::string& agentName,
+                            const armem::Time& timestamp)
     {
         std::lock_guard g{memoryWriterMutex};
 
-        const auto result =
-            memoryWriter.addSegment(properties.memoryName, agentName);
+        const auto result = memoryWriter.addSegment(constants::memoryName, agentName);
+
+        ARMARX_VERBOSE << "Storing scan with " << laserScan.size() << " elements";
 
         if (not result.success)
         {
@@ -65,26 +68,24 @@ namespace armarx::armem::vision::laser_scans::client
             return false;
         }
 
-        const auto iceTimestamp = Time(Duration::MicroSeconds(timestamp));
 
         const auto providerId = armem::MemoryID(result.segmentID);
-        const auto entityID =
-            providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+        const auto entityID = providerId.withEntityName(frame).withTimestamp(timestamp);
 
         armem::EntityUpdate update;
         update.entityID = entityID;
 
         arondto::LaserScanStamped aronSensorData;
         // currently only sets the header
-        toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
+        toAron(laserScan, timestamp, frame, agentName, aronSensorData);
 
         auto dict = aronSensorData.toAron();
         dict->addElement("scan", toAron(laserScan));
 
         update.instancesData = {dict};
-        update.timeCreated   = iceTimestamp;
+        update.timeCreated = timestamp;
 
-        ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
         ARMARX_DEBUG << updateResult;
@@ -97,4 +98,4 @@ namespace armarx::armem::vision::laser_scans::client
         return updateResult.success;
     }
 
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
similarity index 85%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
index b4f7502ef9493692fc82bf79740d2367fe165d4a..6a75f24b53d13c0ba2d885c588b5c3e944388a57 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
@@ -27,11 +27,11 @@
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
 #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
 
 
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
 {
 
     /**
@@ -48,7 +48,6 @@ namespace armarx::armem::vision::laser_scans::client
     class Writer
     {
     public:
-
         Writer(armem::client::MemoryNameSystem& memoryNameSystem);
         virtual ~Writer();
 
@@ -60,13 +59,12 @@ namespace armarx::armem::vision::laser_scans::client
         // void connect() override;
 
         /// to be called in Component::addPropertyDefinitions
-        void registerPropertyDefinitions(
-            armarx::PropertyDefinitionsPtr& def) /*override*/;
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
 
         bool storeSensorData(const LaserScan& laserScan,
                              const std::string& frame,
                              const std::string& agentName,
-                             const std::int64_t& timestamp);
+                             const armem::Time& timestamp);
 
     private:
         armem::client::MemoryNameSystem& memoryNameSystem;
@@ -75,14 +73,11 @@ namespace armarx::armem::vision::laser_scans::client
         // Properties
         struct Properties
         {
-            std::string memoryName      = "Vision";
-            std::string coreSegmentName = "LaserScans";
         } properties;
 
         std::mutex memoryWriterMutex;
 
         const std::string propertyPrefix = "mem.vision.laser_scans.";
-
     };
 
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/constants.h b/source/RobotAPI/libraries/armem_laser_scans/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb934187159e74e0c13a8a0bc68c391d200c762b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/constants.h
@@ -0,0 +1,33 @@
+/**
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+
+namespace armarx::armem::laser_scans::constants
+{
+    const inline std::string memoryName = "LaserScans";
+
+    // core segments
+    const inline std::string laserScanCoreSegment = "LaserScans";
+
+} // namespace armarx::armem::vision::constants
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bb28ee2fe2cc652d8448be683b52fb810795295e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -0,0 +1,323 @@
+#include "Visu.h"
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/ColorMap.h>
+#include <algorithm>
+#include <exception>
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
+
+#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/armem_laser_scans/types.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h>
+
+namespace armarx::armem::server::laser_scans
+{
+
+    Visu::Visu()
+    {
+        Logging::setTag("Visu");
+    }
+
+
+    void
+    Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(
+            p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+        defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
+        defs->optional(p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
+    }
+
+
+    void
+    Visu::init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader)
+    {
+        this->coreSegment = coreSegment;
+        this->virtualRobotReader = virtualRobotReader;
+    }
+
+
+    void
+    Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+    {
+        this->arviz = arviz;
+        if (debugObserver)
+        {
+            bool batchMode = true;
+            this->debugObserver = DebugObserverHelper("LaserScansMemory", debugObserver, batchMode);
+        }
+
+        if (updateTask)
+        {
+            updateTask->stop();
+            updateTask->join();
+            updateTask = nullptr;
+        }
+        updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
+        updateTask->start();
+    }
+
+
+    void
+    Visu::visualizeRun()
+    {
+        CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+        while (updateTask and not updateTask->isStopped())
+        {
+            if (p.enabled)
+            {
+                const Time timestamp = Time::Now();
+                ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
+
+                try
+                {
+                    visualizeOnce(timestamp);
+                }
+                catch (const std::exception& e)
+                {
+                    ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
+                }
+                catch (...)
+                {
+                    ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
+                }
+
+                if (debugObserver.has_value())
+                {
+                    debugObserver->sendDebugObserverBatch();
+                }
+            }
+            cycle.waitForCycleDuration();
+        }
+    }
+
+    void
+    Visu::visualizeScan(const std::vector<Eigen::Vector3f>& points,
+                        const std::string& sensorName,
+                        const std::string& agentName, const viz::Color& color)
+    {
+        viz::PointCloud pointCloud("laser_scan");
+
+        ARMARX_VERBOSE << "Point cloud with " << points.size() << " points";
+
+        for (const auto& point : points)
+        {
+            pointCloud.addPoint(point.x(), point.y(), point.z(), color);
+        }
+
+        pointCloud.pointSizeInPixels(3);
+
+        viz::Layer l = arviz.layer(agentName + "/" + sensorName);
+        l.add(pointCloud);
+
+        arviz.commit(l);
+    }
+
+    std::vector<Eigen::Vector3f>
+    convertScanToGlobal(const armem::laser_scans::LaserScanStamped& scan,
+                        const Eigen::Isometry3f& global_T_sensor)
+    {
+        auto scanCartesian =
+            armarx::armem::laser_scans::util::toCartesian<Eigen::Vector3f>(scan.data);
+
+        for (auto& point : scanCartesian)
+        {
+            point = global_T_sensor * point;
+        }
+
+        return scanCartesian;
+    }
+
+
+    // void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
+    // {
+    //     coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
+    //     {
+    //         getLatestObjectPoses(provSegment, out);
+    //     });
+    // }
+
+
+    // void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
+    // {
+    //     provSegment.forEachEntity([&out](const wm::Entity & entity)
+    //     {
+    //         if (!entity.empty())
+    //         {
+    //             ObjectPose pose = getLatestObjectPose(entity);
+    //             // Try to insert. Fails and returns false if an entry already exists.
+    //             const auto [it, success] = out.insert({pose.objectID, pose});
+    //             if (!success)
+    //             {
+    //                 // An entry with that ID already exists. We keep the newest.
+    //                 if (it->second.timestamp < pose.timestamp)
+    //                 {
+    //                     it->second = pose;
+    //                 }
+    //             }
+    //         }
+    //     });
+    // }
+
+
+    // void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
+    // {
+    //     entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
+    //     {
+    //         arondto::ObjectInstance dto;
+    //         dto.fromAron(instance.data());
+
+    //         fromAron(dto, out);
+    //     });
+    // }
+
+    std::map<std::string, armem::laser_scans::LaserScanStamped>
+    Visu::getCurrentLaserScans()
+    {
+        ARMARX_CHECK_NOT_NULL(coreSegment);
+
+        const auto convert = [this](const wm::EntityInstance& entityInstance)
+            -> ::armarx::armem::laser_scans::LaserScanStamped
+        {
+            const std::optional<armarx::armem::laser_scans::arondto::LaserScanStamped> dto =
+                tryCast<armarx::armem::laser_scans::arondto::LaserScanStamped>(entityInstance);
+
+            ARMARX_CHECK(dto.has_value());
+
+            ::armarx::armem::laser_scans::LaserScanStamped laserScanStamped;
+            fromAron(dto.value(), laserScanStamped);
+
+            const auto ndArrayNavigator =
+                aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("scan"));
+
+            ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+            laserScanStamped.data =
+                armarx::armem::laser_scans::fromAron<LaserScanStep>(ndArrayNavigator);
+
+            ARMARX_VERBOSE << "Number of steps: " << laserScanStamped.data.size();
+
+            return laserScanStamped;
+        };
+
+        std::map<std::string, armem::laser_scans::LaserScanStamped> scans;
+
+        const auto applyToInstance = [&](const wm::EntityInstance& instance)
+        {
+            const auto scan = convert(instance);
+            scans[instance.id().providerSegmentName + "/" + instance.id().entityName] = scan;
+        };
+
+        const auto applyToEntity = [&](const wm::Entity& entity)
+        {
+            if (entity.empty())
+            {
+                return;
+            }
+
+            const auto& snapshot = entity.getLatestSnapshot();
+
+            snapshot.forEachInstance(applyToInstance);
+        };
+
+        const auto applyToProviderSegment = [&](const auto& providerSegment)
+        { providerSegment.forEachEntity(applyToEntity); };
+
+        coreSegment->forEachProviderSegment(applyToProviderSegment);
+
+        ARMARX_VERBOSE << scans.size() << " scans";
+        return scans;
+    }
+
+    void
+    Visu::visualizeOnce(const Time& timestamp)
+    {
+        std::map<std::string, armem::laser_scans::LaserScanStamped> currentLaserScans =
+            getCurrentLaserScans();
+
+        int i = 0;
+
+        for (const auto& [provider, scan] : currentLaserScans)
+        {
+            ARMARX_VERBOSE << "Visualizing `" << provider << "`";
+
+            const auto global_T_sensor = [&]() ->  Eigen::Isometry3f{
+
+                const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
+                if(not robot)
+                {
+                    ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`" << "not available";
+                    return Eigen::Isometry3f::Identity();
+                }
+
+                const auto sensorNode = robot->getRobotNode(scan.header.frame);
+                ARMARX_CHECK_NOT_NULL(sensorNode) << "No robot node `" << scan.header.frame
+                                                << "` for robot `" << scan.header.agent << "`";
+
+                ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is " << sensorNode->getGlobalPosition();
+                return Eigen::Isometry3f{sensorNode->getGlobalPose()};
+            }();
+
+            const std::vector<Eigen::Vector3f> points = convertScanToGlobal(scan, global_T_sensor);
+
+            const auto color = [&]() -> simox::Color{ 
+                if(p.uniformColor)
+                {
+                    return simox::Color::red();
+                }
+                
+                return simox::color::GlasbeyLUT::at(i++);
+            }();
+
+            visualizeScan(points, scan.header.frame, scan.header.agent, color);
+        }
+    }
+
+    VirtualRobot::RobotPtr
+    Visu::getSynchronizedRobot(const std::string& name, const DateTime& timestamp)
+    {
+        if (robots.count(name) > 0)
+        {
+            return robots.at(name);
+        }
+
+        ARMARX_CHECK_NOT_NULL(virtualRobotReader);
+        const auto robot = virtualRobotReader->getRobot(name, timestamp, 
+        VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+        if(robot)
+        {
+            robots[name] = robot;
+        }else
+        {
+            return nullptr;
+        }
+
+        if(not virtualRobotReader->synchronizeRobot(*robot, timestamp))
+        {
+            ARMARX_VERBOSE << "Faield to synchronize robot `" << name << "`";
+        }
+
+        return robots.at(name);
+    }
+
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..23b040637d98c2ac1af0a300e9d29fc772c206dc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
@@ -0,0 +1,95 @@
+/*
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <optional>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+
+
+namespace armarx::armem::server::laser_scans
+{
+
+    /**
+     * @brief Models decay of object localizations by decreasing the confidence
+     * the longer the object was not localized.
+     */
+    class Visu : public armarx::Logging
+    {
+    public:
+        Visu();
+
+
+        void defineProperties(armarx::PropertyDefinitionsPtr defs,
+                              const std::string& prefix = "visu.");
+        void init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader);
+        void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
+
+
+    private:
+        void visualizeRun();
+        void visualizeOnce(const Time& timestamp);
+
+
+        viz::Client arviz;
+        std::optional<DebugObserverHelper> debugObserver;
+
+        struct Properties
+        {
+            bool enabled = true;
+            bool uniformColor = false;
+            float frequencyHz = 5;
+        } p;
+
+
+        SimpleRunningTask<>::pointer_type updateTask;
+
+        armem::robot_state::VirtualRobotReader* virtualRobotReader;
+
+        std::map<std::string, VirtualRobot::RobotPtr> robots;
+
+        VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
+                                                    const DateTime& timestamp);
+
+        void visualizeScan(const std::vector<Eigen::Vector3f>& points,
+                           const std::string& sensorName,
+                           const std::string& agentName,
+                           const viz::Color& color);
+
+        std::map<std::string, armem::laser_scans::LaserScanStamped> getCurrentLaserScans();
+
+        const wm::CoreSegment* coreSegment;
+    };
+
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/types.h b/source/RobotAPI/libraries/armem_laser_scans/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..34f58e01988301e7947de008d63a2865d3324382
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/types.h
@@ -0,0 +1,46 @@
+/*
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+namespace armarx::armem::laser_scans
+{
+    struct SensorHeader
+    {
+        std::string agent;
+        std::string frame;
+        armem::Time timestamp;
+    };
+
+    struct LaserScanStamped
+    {
+        SensorHeader header;
+        LaserScan data;
+    };
+
+} // namespace armarx::armem::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h b/source/RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h
new file mode 100644
index 0000000000000000000000000000000000000000..11e0ee6e8a3f071117aa938bf3e5a23cc1ef6935
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h
@@ -0,0 +1,61 @@
+/*
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::armem::laser_scans::util
+{
+
+    template <typename EigenVectorT>
+    EigenVectorT
+    toCartesian(const LaserScanStep& laserScanStep)
+    {
+        EigenVectorT point = EigenVectorT::Identity();
+
+        point.x() = laserScanStep.distance * std::cos(laserScanStep.angle);
+        point.y() = laserScanStep.distance * std::sin(laserScanStep.angle);
+
+        return point;
+    }
+
+    template <typename EigenVectorT>
+    std::vector<EigenVectorT>
+    toCartesian(const armarx::LaserScan& laserScan)
+    {
+        std::vector<EigenVectorT> points;
+        points.reserve(laserScan.size());
+
+        std::transform(laserScan.begin(),
+                       laserScan.end(),
+                       std::back_inserter(points),
+                       [](const LaserScanStep& pt) { return toCartesian<EigenVectorT>(pt); });
+
+        return points;
+    }
+
+} // namespace armarx::armem::laser_scans::util
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 404dc1f33dc6f300d52970386be46665ca6971e5..8c1f8dfb0ad4f6f833d27191588fc8d766998235 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -77,8 +77,9 @@ namespace armarx::armem::robot_state
             return nullptr;
         }
 
-        const std::string xmlFilename =
-            ArmarXDataPath::resolvePath(description->xml.serialize().path);
+        const std::string xmlFilename = description->xml.toSystemPath();
+        ARMARX_CHECK(std::filesystem::exists(xmlFilename)) << xmlFilename;
+
         ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '"
                         << xmlFilename << "'";
 
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 1607132e51d9370e3ea746193d835f265bf529ff..ff413dd475886dc1436a4a492b28668c17e167b7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -110,6 +110,8 @@ namespace armarx::armem::server::robot_state
                 Eigen::Affine3f from = pose;
                 Eigen::Affine3f to = pose * frame;
 
+                pose = to;
+
                 layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
             }
         }
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 8f7d631a1b275a73607901892cfbfc290ae10bf9..e285967fdcf1d5df56dd483c2a298a1b908d7116 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -133,7 +133,7 @@ namespace armarx::armem::server::robot_state::description
             }
         });
 
-        ARMARX_INFO << deactivateSpam(60) <<  "Number of known robot descriptions: " << robotDescriptions.size();
+        ARMARX_VERBOSE << deactivateSpam(60) <<  "Number of known robot descriptions: " << robotDescriptions.size();
         return robotDescriptions;
     }
 
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 3cadfeccb0a86bd6672dd1af8fc072086dca4fa4..3fbf097937386a7d4973b55a22f76419d3f5d5eb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -106,8 +106,8 @@ namespace armarx::armem::server::robot_state::localization
             frames.emplace(robotName, result.transforms);
         }
 
-        ARMARX_INFO << deactivateSpam(60)
-                    << "Number of known robot pose chains: " << frames.size();
+        ARMARX_VERBOSE << deactivateSpam(60)
+                       << "Number of known robot pose chains: " << frames.size();
 
         return frames;
     }
@@ -153,8 +153,8 @@ namespace armarx::armem::server::robot_state::localization
             }
         }
 
-        ARMARX_INFO << deactivateSpam(60)
-                    << "Number of known robot poses: " << robotGlobalPoses.size();
+        ARMARX_VERBOSE << deactivateSpam(60)
+                       << "Number of known robot poses: " << robotGlobalPoses.size();
 
         return robotGlobalPoses;
     }
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index f8bbe5ec1db7641c9f634ff51ba6416e1d2dbb81..4d179ca04bddc9e1eb11ebb5b08aea520e520d83 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -13,10 +13,9 @@ armarx_add_library(
         aroncommon
         # System / External
         Eigen3::Eigen
+        armem_laser_scans
     HEADERS
         ./aron_conversions.h
-        ./client/laser_scans/Reader.h
-        ./client/laser_scans/Writer.h
         ./client/occupancy_grid/Reader.h
         ./client/occupancy_grid/Writer.h
         ./client/laser_scanner_features/Reader.h
@@ -25,8 +24,6 @@ armarx_add_library(
         constants.h
     SOURCES
         ./aron_conversions.cpp
-        ./client/laser_scans/Reader.cpp
-        ./client/laser_scans/Writer.cpp
         ./client/occupancy_grid/Reader.cpp
         ./client/occupancy_grid/Writer.cpp
         ./client/laser_scanner_features/Reader.cpp
@@ -38,7 +35,6 @@ armarx_enable_aron_file_generation_for_target(
     TARGET_NAME
         "${LIB_NAME}"
     ARON_FILES
-        aron/LaserScan.xml
         aron/LaserScannerFeatures.xml
         aron/OccupancyGrid.xml
 )
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 6b0ea82d8ab18f6aa885d075949caa75b288d279..814ec54638ae165734b5b5e629684328d8085277 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -6,7 +6,7 @@
 
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
 #include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/aron/converter/common/Converter.h>
@@ -18,37 +18,6 @@
 namespace armarx::armem::vision
 {
 
-    /************ fromAron ************/
-    SensorHeader
-    fromAron(const arondto::SensorHeader& aronSensorHeader)
-    {
-        return {.agent = aronSensorHeader.agent,
-                .frame = aronSensorHeader.frame,
-                .timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
-    }
-
-    void
-    fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
-    {
-        laserScan.header = fromAron(aronLaserScan.header);
-        // laserScan.data = fromAron(aronLaserScan.data);
-    }
-
-    void
-    fromAron(const arondto::LaserScanStamped& aronLaserScan,
-             LaserScan& laserScan,
-             std::int64_t& timestamp,
-             std::string& frame,
-             std::string& agentName)
-    {
-        const auto header = fromAron(aronLaserScan.header);
-
-        // laserScan = fromAron(aronLaserScan.data);
-
-        timestamp = header.timestamp.toMicroSecondsSinceEpoch();
-        frame = header.frame;
-        agentName = header.agent;
-    }
 
     /************ toAron ************/
 
@@ -62,40 +31,7 @@ namespace armarx::armem::vision
     {
         return timestamp.toMicroSecondsSinceEpoch();
     }
-
-    arondto::SensorHeader
-    toAron(const SensorHeader& sensorHeader)
-    {
-        arondto::SensorHeader aronSensorHeader;
-
-        aron::toAron(aronSensorHeader.agent, sensorHeader.agent);
-        aron::toAron(aronSensorHeader.frame, sensorHeader.frame);
-        aron::toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
-
-        return aronSensorHeader;
-    }
-
-    void
-    toAron(const LaserScanStamped& laserScanStamped,
-           arondto::LaserScanStamped& aronLaserScanStamped)
-    {
-        aronLaserScanStamped.header = toAron(laserScanStamped.header);
-        // toAron(laserScanStamped.data, aronLaserScanStamped.data);
-    }
-
-    void
-    toAron(const LaserScan& laserScan,
-           const armem::Time& timestamp,
-           const std::string& frame,
-           const std::string& agentName,
-           arondto::LaserScanStamped& aronLaserScanStamped)
-    {
-        const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
-
-        const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
-
-        toAron(laserScanStamped, aronLaserScanStamped);
-    }
+    
 
     void
     toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
index e9cf78c698b31345a9fcc17262bb7a23a25cd7fc..216088b410231671dd49a22a95f9fa2b6ae9362b 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
@@ -40,7 +40,7 @@
 #include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 #include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
 #include <RobotAPI/libraries/armem_vision/aron_conversions.h>
 #include <RobotAPI/libraries/armem_vision/types.h>
 
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
index 5746bffa1880c3031e183acf5cecd74f2d6d515a..0b24561dd5507efeef77793a5285b5a3eb2b00c9 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
@@ -2,7 +2,7 @@
 
 #include "RobotAPI/libraries/armem_vision/constants.h"
 #include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
 #include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
 #include <RobotAPI/libraries/armem_vision/aron_conversions.h>
 
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index 91505d40e5ea0046fc2419ac532db85010062011..a98495d192583b101299c8b6b0b365f569427a38 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -42,7 +42,6 @@
 #include <RobotAPI/libraries/armem/client/query/selectors.h>
 #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
 #include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
 #include <RobotAPI/libraries/armem_vision/aron_conversions.h>
 #include <RobotAPI/libraries/armem_vision/types.h>
 
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index 85322d188ce8fdbd9b065ce476e281408d38f254..fe584e3dc235a19333fd9b174cfab390c1a5d435 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -31,18 +31,6 @@
 namespace armarx::armem::vision
 {
 
-    struct SensorHeader
-    {
-        std::string agent;
-        std::string frame;
-        armem::Time timestamp;
-    };
-
-    struct LaserScanStamped
-    {
-        SensorHeader header;
-        LaserScan data;
-    };
 
     // template<typename _ValueT = float>
     struct OccupancyGrid