Add profiling, improve runtime

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

@@ -91,33 +91,65 @@ namespace armarx::armem::server::robot_state::proprioception
 
     std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
     {
+        namespace adn = aron::datanavigator;
+
         std::unordered_map<std::string, std::map<std::string, float>> jointMap;
 
+        TIMING_START(tRobotJointPositionsLock);
         std::lock_guard g{memoryMutex};
+        TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_VERBOSE);
 
+        TIMING_START(tProviders);
         for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
         {
+            TIMING_START(tEntities);
+            int i = 0;
             for (const auto& [name, entity] :  provSeg.entities())
             {
+                TIMING_START(tTryCatch);
                 try
                 {
+                    TIMING_START(tGetLatestInstance);
                     const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-                    const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
-
-                    if (not jointState)
+                    TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_VERBOSE);
+
+                    TIMING_START(tRobotJointPositionsTryCast);
+                    // tryCast is too slow.
+                    // const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+                    aron::datanavigator::DictNavigatorPtr data = entityInstance.data();
+                    // Avoid throwing an exception.
+                    if (!(data->hasElement("name") && data->hasElement("position")))
                     {
-                        // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
                         continue;
                     }
-                    jointMap[robotName].emplace(jointState->name, jointState->position);
+                    armarx::armem::arondto::JointState jointState;
+                    jointState.name = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue();
+                    jointState.position = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue();
+                    TIMING_END_COMMENT_STREAM(tRobotJointPositionsTryCast, "tRobotJointPositionsTryCast " + std::to_string(i), ARMARX_VERBOSE);
 
+                    TIMING_START(tEmplace);
+                    if (jointState.name.size() > 0)
+                    {
+                        jointMap[robotName].emplace(jointState.name, jointState.position);
+                    }
+                    else
+                    {
+                        // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+                    }
+                    TIMING_END_COMMENT_STREAM(tEmplace, "tEmplace " + std::to_string(i), ARMARX_VERBOSE);
                 }
-                catch (...) // empty history etc
+                catch (const armem::error::EntityHistoryEmpty&)
+                {
+                }
+                catch (const armem::error::MissingEntry&)
                 {
-
                 }
+                TIMING_END_STREAM(tTryCatch, ARMARX_VERBOSE);
+                ++i;
             }
+            TIMING_END_STREAM(tEntities, ARMARX_VERBOSE);
         }
+        TIMING_END_STREAM(tProviders, ARMARX_VERBOSE);
 
         ARMARX_INFO << deactivateSpam(10) <<  "Number of known robot joint maps: " << jointMap.size();