source/RobotAPI/interface/units/LaserScannerUnit.ice

@@ -46,18 +46,18 @@ module armarx
 	**/
     struct LaserScanStep
     {
-	float angle;
-	float distance;
-//	float intensity;
+        float angle;
+        float distance;
+        float intensity;
     };
 
     struct LaserScannerInfo
     {
-	string device;
-	string frame;
-	float minAngle;
-	float maxAngle;
-	float stepSize;
+        string device;
+        string frame;
+        float minAngle;
+        float maxAngle;
+        float stepSize;
     };
 
     sequence<LaserScanStep> LaserScan;
@@ -82,4 +82,3 @@ module armarx
     };
 
 };
-

source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp

@@ -2,56 +2,115 @@
 
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
 
 #include <RobotAPI/interface/components/RobotHealthInterface.h>
 
 namespace armarx::plugins
 {
-    void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
-                                                             const RobotHealthHeartbeatArgs& args)
+
+    void
+    HeartbeatComponentPlugin::signUp(const std::string& identifier,
+                                     const std::vector<std::string>& tags,
+                                     const std::string& description)
+    {
+        RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+        argsCopy.description = description;
+        argsCopy.identifier = identifier;
+        argsCopy.tags = tags;
+        signUp(argsCopy);
+    }
+
+    void
+    HeartbeatComponentPlugin::signUp(const std::string& identifier,
+                                     const armarx::core::time::Duration& warning,
+                                     const armarx::core::time::Duration& error,
+                                     const std::vector<std::string>& tags,
+                                     const std::string& description)
     {
-        channelHeartbeatConfig.emplace(channel, args);
+        RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+        argsCopy.description = description;
+        argsCopy.tags = tags;
+        argsCopy.identifier = identifier;
+        toIce(argsCopy.maximumCycleTimeWarning, warning);
+        toIce(argsCopy.maximumCycleTimeError, error);
+
+        ARMARX_TRACE;
+        signUp(argsCopy);
     }
 
-    void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, const std::string& message)
+    void
+    HeartbeatComponentPlugin::signUp(const armarx::core::time::Duration& warning,
+                                     const armarx::core::time::Duration& error,
+                                     const std::vector<std::string>& tags,
+                                     const std::string& description)
     {
-        auto args = heartbeatArgs;
-        args.message = message;
-        configureHeartbeatChannel(channel, args);
+        RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+        argsCopy.description = description;
+        argsCopy.tags = tags;
+        toIce(argsCopy.maximumCycleTimeWarning, warning);
+        toIce(argsCopy.maximumCycleTimeError, error);
+        // argsCopy.requiredByDefault = required;
+        signUp(argsCopy);
     }
 
-    void HeartbeatComponentPlugin::heartbeat()
+    void
+    HeartbeatComponentPlugin::signUp(const RobotHealthHeartbeatArgs& args)
     {
+        ARMARX_TRACE;
+        ARMARX_CHECK_NOT_NULL(robotHealthComponentPrx);
 
-        if (robotHealthTopic)
+        if (args.identifier.empty())
         {
-            robotHealthTopic->heartbeat(componentName, heartbeatArgs);
-        } else
+            RobotHealthHeartbeatArgs argsCopy = args;
+            argsCopy.identifier = parent().getName();
+            robotHealthComponentPrx->signUp(argsCopy);
+        }
+        else
         {
-            ARMARX_WARNING << "No robot health topic available!";
+            // add component name prefix to identifier
+            RobotHealthHeartbeatArgs argsCopy = args;
+            argsCopy.identifier = parent().getName() + "_" + argsCopy.identifier;
+            robotHealthComponentPrx->signUp(argsCopy);
         }
     }
 
-    void HeartbeatComponentPlugin::heartbeat(const std::string& channel)
+    void
+    HeartbeatComponentPlugin::heartbeat()
     {
-        const auto argsIt = channelHeartbeatConfig.find(channel);
-        ARMARX_CHECK(argsIt != channelHeartbeatConfig.end()) << "heartbeat() called for unknown channel '" << channel
-                                                             << "'."
-                                                             << "You must register the config using configureHeartbeatChannel(channel) first!";
-
-        const auto& args = argsIt->second;
+        if (robotHealthComponentPrx)
+        {
+            armarx::core::time::dto::DateTime now;
+            armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+            robotHealthComponentPrx->heartbeat(parent().getName(), now);
+        }
+        else
+        {
+            ARMARX_WARNING << "No robot health proxy available!";
+        }
+    }
 
-        if (robotHealthTopic)
+    void
+    HeartbeatComponentPlugin::heartbeatOnChannel(const std::string& channelName)
+    {
+        if (robotHealthComponentPrx)
         {
-            robotHealthTopic->heartbeat(componentName + "_" + channel, args);
-        } else
+            armarx::core::time::dto::DateTime now;
+            armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+            robotHealthComponentPrx->heartbeat(parent().getName() + "_" + channelName, now);
+        }
+        else
         {
-            ARMARX_WARNING << "No robot health topic available!";
+            ARMARX_WARNING << "No robot health proxy available!";
         }
     }
 
-    void HeartbeatComponentPlugin::preOnInitComponent()
+    void
+    HeartbeatComponentPlugin::preOnInitComponent()
     {
+        // defaultHeartbeatArgs.requiredByDefault = true;
+
         //        if (topicName.empty())
         //        {
         //            parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
@@ -59,33 +118,42 @@ namespace armarx::plugins
         //        parent<Component>().offeringTopic(topicName);
     }
 
-    void HeartbeatComponentPlugin::postOnInitComponent()
+    void
+    HeartbeatComponentPlugin::postOnInitComponent()
     {
+        // set default args
+        auto warn = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarningMS);
+        auto err = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrorMS);
+        armarx::core::time::toIce(defaultHeartbeatArgs.maximumCycleTimeWarning, warn);
+        armarx::core::time::toIce(defaultHeartbeatArgs.maximumCycleTimeError, err);
     }
 
-    void HeartbeatComponentPlugin::preOnConnectComponent()
+    void
+    HeartbeatComponentPlugin::postOnConnectComponent()
     {
-        //        robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
-        componentName = parent<Component>().getName();
+        ARMARX_CHECK_NOT_NULL(robotHealthComponentPrx);
     }
 
-    void HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+    void
+    HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
     {
-        if (!properties->hasDefinition(makePropertyName(topicPropertyName)))
+        if (!properties->hasDefinition(makePropertyName(healthPropertyName)))
         {
-            properties->topic(robotHealthTopic, topicName, topicPropertyName,
-                              "Name of the topic the DebugObserver listens on");
+            properties->component(
+                robotHealthComponentPrx, "RobotHealth", healthPropertyName, "Name of the robot health component.");
         }
 
         if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
         {
-            properties->required(heartbeatArgs.maximumCycleTimeWarningMS, maximumCycleTimeWarningMSPropertyName,
+            properties->optional(p.maximumCycleTimeWarningMS,
+                                 maximumCycleTimeWarningMSPropertyName,
                                  "maximum cycle time before warning is emitted");
         }
 
         if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
         {
-            properties->required(heartbeatArgs.maximumCycleTimeErrorMS, maximumCycleTimeErrorMSPropertyName,
+            properties->optional(p.maximumCycleTimeErrorMS,
+                                 maximumCycleTimeErrorMSPropertyName,
                                  "maximum cycle time before error is emitted");
         }
     }

source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h

@@ -22,6 +22,8 @@
 #pragma once
 
 #include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/time/Duration.h>
 
 #include <RobotAPI/interface/components/RobotHealthInterface.h>
 
@@ -34,21 +36,33 @@ namespace armarx::plugins
         using ComponentPlugin::ComponentPlugin;
 
         /**
-         * @brief Configures a heartbeat subchannel.
-         *
-         * @param channel Identifier of the heartbeat channel
-         * @param args Configuration of this channel's heartbeat properties
+         * @brief register component to heartbeat
          */
-        void configureHeartbeatChannel(const std::string& channel, const std::string& message);
+        void signUp(const std::string& channelName = "",
+                    const std::vector<std::string>& aliases = {},
+                    const std::string& description = "");
 
         /**
-         * @brief Configures a heartbeat subchannel.
-         *
-         * @param channel Identifier of the heartbeat channel
-         * @param args Configuration of this channel's heartbeat properties
+         * @brief register component to heartbeat
+         */
+        void signUp(const armarx::core::time::Duration& warning,
+                    const armarx::core::time::Duration& error,
+                    const std::vector<std::string>& aliases = {},
+                    const std::string& description = "");
+
+        /**
+         * @brief register component to heartbeat
+         */
+        void signUp(const std::string& channelName,
+                    const armarx::core::time::Duration& warning,
+                    const armarx::core::time::Duration& error,
+                    const std::vector<std::string>& aliases = {},
+                    const std::string& description = "");
+
+        /**
+         * @brief register component to heartbeat, possibly with different component name
          */
-        void configureHeartbeatChannel(const std::string& channel,
-                                       const RobotHealthHeartbeatArgs& args);
+        void signUp(const RobotHealthHeartbeatArgs& args);
 
         /**
          * @brief Sends out a heartbeat using the default config
@@ -63,35 +77,33 @@ namespace armarx::plugins
          *
          * @param channel Identifier of the heartbeat channel
          */
-        void heartbeat(const std::string& channel);
+        void heartbeatOnChannel(const std::string& channelName);
 
     protected:
         void preOnInitComponent() override;
         void postOnInitComponent() override;
-        void preOnConnectComponent() override;
+        // void preOnConnectComponent() override;
+        void postOnConnectComponent() override;
 
         void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
 
     private:
-        //! heartbeat topic name (outgoing)
-        std::string topicName{"RobotHealthTopic"};
-
-        //! name of this component used as identifier for heartbeats
-        std::string componentName;
+        RobotHealthComponentInterfacePrx robotHealthComponentPrx;
 
         //
-        static constexpr auto topicPropertyName = "heartbeat.TopicName";
+        static constexpr auto healthPropertyName = "heartbeat.ComponentName";
         static constexpr auto maximumCycleTimeWarningMSPropertyName =
             "heartbeat.maximumCycleTimeWarningMS";
         static constexpr auto maximumCycleTimeErrorMSPropertyName =
             "heartbeat.maximumCycleTimeErrorMS";
 
-        RobotHealthInterfacePrx robotHealthTopic;
+        struct Properties
+        {
+            long maximumCycleTimeWarningMS = 100; // [ms]
+            long maximumCycleTimeErrorMS = 200; // [ms]
+        } p;
 
         //! default config used in heartbeat(), set via properties
-        RobotHealthHeartbeatArgs heartbeatArgs;
-
-        //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...)
-        std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig;
+        RobotHealthHeartbeatArgs defaultHeartbeatArgs;
     };
 } // namespace armarx::plugins

source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp

@@ -36,14 +36,14 @@
 namespace dn = armarx::aron::data;
 namespace tn = armarx::aron::type;
 
-
 namespace ArMemGuiTest
 {
     struct Fixture
     {
     };
 
-    void test_sanitize(const std::string& in, const std::string& expected)
+    void
+    test_sanitize(const std::string& in, const std::string& expected)
     {
         const std::string out = armarx::armem::gui::instance::sanitizeTypeName(in);
         BOOST_TEST_CONTEXT("in = '" << in << "'")
@@ -51,13 +51,11 @@ namespace ArMemGuiTest
             BOOST_CHECK_EQUAL(out, expected);
         }
     }
-}
-
+} // namespace ArMemGuiTest
 
 
 BOOST_FIXTURE_TEST_SUITE(ArMemGuiTest, ArMemGuiTest::Fixture)
 
-
 BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_int)
 {
     test_sanitize(tn::Int().getFullName(), "Int");
@@ -72,7 +70,8 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_float)
 
 BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_dict)
 {
-    tn::Dict dict(std::make_shared<tn::Float>());
+    tn::Dict dict;
+    dict.setAcceptedType(std::make_shared<tn::Float>());
     test_sanitize(dict.getFullName(), "Dict<Float>");
 
     test_sanitize(dn::Dict().getFullName(), "Dict");
@@ -80,7 +79,8 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_dict)
 
 BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_list)
 {
-    tn::List list(std::make_shared<tn::Long>());
+    tn::List list;
+    list.setAcceptedType(std::make_shared<tn::Long>());
     test_sanitize(list.getFullName(), "List<Long>");
 
     test_sanitize(dn::List().getFullName(), "List");
@@ -88,20 +88,24 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_list)
 
 BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_object)
 {
-    tn::Object obj("namespace::MyObjectName");
+    tn::Object obj;
+    obj.setObjectName("namespace::MyObjectName");
     test_sanitize(obj.getFullName(), "MyObjectName    (namespace)");
 }
 
 BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_tuple)
 {
     std::vector<tn::VariantPtr> ac = {std::make_shared<tn::Int>(), std::make_shared<tn::Float>()};
-    tn::Tuple type(ac);
+    tn::Tuple type;
+    type.setAcceptedTypes(ac);
     test_sanitize(type.getFullName(), "Tuple<Int, Float>");
 }
 
 BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_pair)
 {
-    tn::Pair type(std::make_shared<tn::Int>(), std::make_shared<tn::Float>());
+    tn::Pair type;
+    type.setFirstAcceptedType(std::make_shared<tn::Int>());
+    type.setSecondAcceptedType(std::make_shared<tn::Float>());
     test_sanitize(type.getFullName(), "Pair<Int, Float>");
 }
 
@@ -115,7 +119,7 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_ndarry)
     }
     {
         dn::NDArray data;
-        data.setShape({ 3, 2, 1});
+        data.setShape({3, 2, 1});
         test_sanitize(data.getFullName(), "NDArray<3, 2, 1, >");
     }
 }

source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp

@@ -80,7 +80,7 @@ namespace armarx::armem::laser_scans::client
         toAron(laserScan, timestamp, frame, agentName, aronSensorData);
 
         auto dict = aronSensorData.toAron();
-        dict->addElement("scan", toAron(laserScan));
+        dict->addElementCopy("scan", toAron(laserScan));
 
         update.instancesData = {dict};
         update.referencedTime = timestamp;

source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp

 #include "Visu.h"
 
-#include <SimoxUtility/color/Color.h>
-#include <SimoxUtility/color/ColorMap.h>
 #include <algorithm>
 #include <exception>
 #include <string>
@@ -10,6 +8,9 @@
 
 #include <SimoxUtility/algorithm/apply.hpp>
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/ColorMap.h>
+#include <SimoxUtility/color/hsv.h>
 #include <SimoxUtility/math/pose.h>
 
 #include <ArmarXCore/core/logging/Logging.h>
@@ -32,33 +33,34 @@
 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.");
+        defs->optional(
+            p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
         defs->optional(p.maxRobotAgeMs,
                        prefix + "maxRobotAgeMs",
                        "Maximum age of robot state before a new one is retrieved in milliseconds.");
+        defs->optional(p.colorByIntensity, "colorByIntensity", "");
     }
 
-
     void
-    Visu::init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader)
+    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)
     {
@@ -79,7 +81,6 @@ namespace armarx::armem::server::laser_scans
         updateTask->start();
     }
 
-
     void
     Visu::visualizeRun()
     {
@@ -114,9 +115,10 @@ namespace armarx::armem::server::laser_scans
     }
 
     void
-    Visu::visualizeScan(const std::vector<Eigen::Vector3f>& points,
+    Visu::visualizeScan(const std::vector<ScanPoint>& points,
                         const std::string& sensorName,
-                        const std::string& agentName, const viz::Color& color)
+                        const std::string& agentName,
+                        const viz::Color& color)
     {
         viz::PointCloud pointCloud("laser_scan");
 
@@ -124,7 +126,30 @@ namespace armarx::armem::server::laser_scans
 
         for (const auto& point : points)
         {
-            pointCloud.addPoint(point.x(), point.y(), point.z(), color);
+
+            // ARMARX_INFO << point.intensity;
+            const viz::Color specificColor = [&point, &color, this]() -> viz::Color
+            {
+                if (p.colorByIntensity)
+                {
+                    Eigen::Vector3f hsv = simox::color::rgb_to_hsv(
+                        Eigen::Vector3f(static_cast<float>(color.r) / 255.f,
+                                        static_cast<float>(color.g) / 255.f,
+                                        static_cast<float>(color.b) / 255.f));
+
+                    // ARMARX_INFO << point.intensity;
+
+                    hsv(2) = std::clamp<float>(point.intensity, 0., 1.);
+
+                    const Eigen::Vector3f rgb = simox::color::hsv_to_rgb(hsv);
+
+                    return viz::Color{rgb(0), rgb(1), rgb(2)};
+                }
+
+                return color;
+            }();
+
+            pointCloud.addPoint(point.point.x(), point.point.y(), point.point.z(), specificColor);
         }
 
         pointCloud.pointSizeInPixels(3);
@@ -135,22 +160,28 @@ namespace armarx::armem::server::laser_scans
         arviz.commit(l);
     }
 
-    std::vector<Eigen::Vector3f>
+    std::vector<ScanPoint>
     convertScanToGlobal(const armem::laser_scans::LaserScanStamped& scan,
                         const Eigen::Isometry3f& global_T_sensor)
     {
-        auto scanCartesian =
+        const auto scanCartesian =
             armarx::armem::laser_scans::util::toCartesian<Eigen::Vector3f>(scan.data);
 
-        for (auto& point : scanCartesian)
+        std::vector<ScanPoint> points;
+        points.reserve(scan.data.size());
+
+        for (std::size_t i = 0; i < scan.data.size(); i++)
         {
-            point = global_T_sensor * point;
+            const auto& point = scanCartesian.at(i);
+            const auto& raw = scan.data.at(i);
+
+            const Eigen::Vector3f pointGlobal = global_T_sensor * point;
+            points.push_back(ScanPoint{.point = pointGlobal, .intensity = raw.intensity});
         }
 
-        return scanCartesian;
+        return points;
     }
 
-
     // void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
     // {
     //     coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
@@ -263,31 +294,35 @@ namespace armarx::armem::server::laser_scans
         {
             ARMARX_VERBOSE << "Visualizing `" << provider << "`";
 
-            const auto global_T_sensor = [&]() ->  Eigen::Isometry3f{
-
+            const auto global_T_sensor = [&]() -> Eigen::Isometry3f
+            {
                 const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
-                if(not robot)
+                if (not robot)
                 {
-                    ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`" << "not available";
+                    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 << "`";
+                                                  << "` for robot `" << scan.header.agent << "`";
 
-                ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is " << sensorNode->getGlobalPosition();
+                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)
+            const std::vector<ScanPoint> points = convertScanToGlobal(scan, global_T_sensor);
+
+            const auto color = [&]() -> simox::Color
+            {
+                if (p.uniformColor)
                 {
                     return simox::Color::red();
                 }
-                
+
                 return simox::color::GlasbeyLUT::at(i++);
             }();
 

source/RobotAPI/libraries/armem_laser_scans/server/Visu.h

@@ -33,14 +33,18 @@
 #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/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.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
 {
+    struct ScanPoint
+    {
+        Eigen::Vector3f point;
+        float intensity;
+    };
 
     /**
      * @brief Models decay of object localizations by decreasing the confidence
@@ -54,7 +58,8 @@ namespace armarx::armem::server::laser_scans
 
         void defineProperties(armarx::PropertyDefinitionsPtr defs,
                               const std::string& prefix = "visu.");
-        void init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader);
+        void init(const wm::CoreSegment* coreSegment,
+                  armem::robot_state::VirtualRobotReader* virtualRobotReader);
         void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
 
 
@@ -70,11 +75,11 @@ namespace armarx::armem::server::laser_scans
         {
             bool enabled = true;
             bool uniformColor = false;
+            bool colorByIntensity = true;
             float frequencyHz = 5;
             int maxRobotAgeMs = 100;
         } p;
 
-
         SimpleRunningTask<>::pointer_type updateTask;
 
         armem::robot_state::VirtualRobotReader* virtualRobotReader;
@@ -84,7 +89,7 @@ namespace armarx::armem::server::laser_scans
         VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
                                                     const DateTime& timestamp);
 
-        void visualizeScan(const std::vector<Eigen::Vector3f>& points,
+        void visualizeScan(const std::vector<ScanPoint>& points,
                            const std::string& sensorName,
                            const std::string& agentName,
                            const viz::Color& color);

source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp

@@ -258,19 +258,19 @@ namespace armarx::armem::common::robot_state::localization
         }
         catch (const armem::error::MissingEntry& missingEntryError)
         {
-            ARMARX_WARNING << missingEntryError.what();
+            ARMARX_VERBOSE << missingEntryError.what();
         }
         catch (const ::armarx::exceptions::local::ExpressionException& ex)
         {
-            ARMARX_WARNING << "Local expression exception: " << ex.what();
+            ARMARX_VERBOSE << "Local expression exception: " << ex.what();
         }
         catch (const ::armarx::LocalException& ex)
         {
-            ARMARX_WARNING << "Local exception: " << ex.what();
+            ARMARX_VERBOSE << "Local exception: " << ex.what();
         }
         catch (...)
         {
-            ARMARX_WARNING << "Unexpected error: " << GetHandledExceptionString();
+            ARMARX_VERBOSE << "Unexpected error: " << GetHandledExceptionString();
         }
 
         return {};