Handle missing kinematic unit

source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp

@@ -5,11 +5,12 @@
 
 #include <IceUtil/Time.h>
 
+#include <ArmarXCore/core/application/properties/PluginAll.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
 
 #include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -56,9 +57,7 @@ namespace armarx::armem::server::robot_state::description
         ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
 
         ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
-
-        coreSegment = &iceMemory.workingMemory->addCoreSegment(
-                          p.coreSegment, arondto::RobotDescription::toAronType());
+        coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::RobotDescription::toAronType());
         coreSegment->setMaxHistorySize(p.maxHistorySize);
     }
 
@@ -79,7 +78,7 @@ namespace armarx::armem::server::robot_state::description
     }
 
 
-    void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+    void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
     {
         const Time now = TimeUtil::GetTime();
 
@@ -99,11 +98,7 @@ namespace armarx::armem::server::robot_state::description
 
         Commit commit;
         commit.updates.push_back(update);
-
-        {
-            // std::lock_guard g{memoryMutex};
-            iceMemory.commit(commit);
-        }
+        iceMemory.commitLocking(commit);
     }
 
 
@@ -111,23 +106,29 @@ namespace armarx::armem::server::robot_state::description
     {
         ARMARX_CHECK_NOT_NULL(robotUnit);
         KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
-        ARMARX_CHECK_NOT_NULL(kinematicUnit);
+        if (kinematicUnit)
+        {
+            const std::string robotName = kinematicUnit->getRobotName();
+            const std::string robotFilename = kinematicUnit->getRobotFilename();
 
-        const std::string robotName = kinematicUnit->getRobotName();
-        const std::string robotFilename = kinematicUnit->getRobotFilename();
+            const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+            const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
 
-        const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
-        const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+            ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
 
-        ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+            const robot::RobotDescription robotDescription
+            {
+                .name = kinematicUnit->getRobotName(),
+                .xml  = {package, kinematicUnit->getRobotFilename()}
+            }; // FIXME
 
-        const robot::RobotDescription robotDescription
+            commitRobotDescription(robotDescription);
+        }
+        else
         {
-            .name = kinematicUnit->getRobotName(),
-            .xml  = {package, kinematicUnit->getRobotFilename()}
-        }; // FIXME
-
-        storeRobotDescription(robotDescription);
+            ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+                           << "\n Cannot commit robot description.";
+        }
     }
 
 
@@ -159,8 +160,7 @@ namespace armarx::armem::server::robot_state::description
                     }
                     else
                     {
-                        ARMARX_WARNING << "Could not convert entity instance to "
-                                       "'RobotDescription'";
+                        ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
                     }
                 }
             }

source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h

@@ -72,7 +72,7 @@ namespace armarx::armem::server::robot_state::description
 
     private:
 
-        void storeRobotDescription(const robot::RobotDescription& robotDescription);
+        void commitRobotDescription(const robot::RobotDescription& robotDescription);
         void updateRobotDescription();
 
         server::MemoryToIceAdapter& iceMemory;

source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp

@@ -5,6 +5,7 @@
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
 
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -59,20 +60,18 @@ namespace armarx::armem::server::robot_state::proprioception
     {
         this->robotUnit = robotUnitPrx;
 
+        std::string providerSegmentName = "Robot";
         KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
-        const std::string providerSegmentName = kinematicUnit->getRobotName();
+        if (kinematicUnit)
+        {
+            providerSegmentName = kinematicUnit->getRobotName();
+        }
+        else
+        {
+            ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+                           << "\n Falling back to provider segment name '" << providerSegmentName << "'.";
+        }
         this->robotUnitProviderID = coreSegment->id().withProviderSegmentName(providerSegmentName);
-
-#if 0
-        // TODO what is the purpose?
-        auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
-        auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
-        auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
-        encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
-        encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
-        //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
-        //encoderEntryType->addMemberType("value", encoderValueType);
-#endif
     }