simplifying robot state and virtual robot reader: only robot name is required...

simplifying robot state and virtual robot reader: only robot name is required as input parameter instead of robot description

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -105,23 +105,23 @@ namespace armarx::armem::robot_state::client::common
     }
 
     bool
-    RobotReader::synchronize(Robot& obj, const armem::Time& timestamp) const
+    RobotReader::synchronize(Robot& robot, const armem::Time& timestamp) const
     {
         const auto tsStartFunctionInvokation = armem::Time::Now();
 
         while (true)
         {
-            auto state = queryState(obj.description, timestamp);
+            auto state = queryState(robot.description.name, timestamp);
 
             if (not state) /* c++20 [[unlikely]] */
             {
-                ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+                ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
 
                 // if the syncTime is non-positive there will be no retry
                 const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
                 if (elapsedTime > syncTimeout)
                 {
-                    ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+                    ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
                     return false;
                 }
 
@@ -129,7 +129,7 @@ namespace armarx::armem::robot_state::client::common
                 Clock::WaitFor(sleepAfterFailure);
             }
 
-            obj.config = std::move(*state);
+            robot.config = std::move(*state);
             return true;
         }
     }
@@ -183,17 +183,17 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<RobotState>
-    RobotReader::queryState(const description::RobotDescription& description,
+    RobotReader::queryState(const std::string& robotName,
                             const armem::Time& timestamp) const
     {
-        std::optional<RobotState> robotState = queryJointState(description, timestamp);
+        std::optional<RobotState> robotState = queryJointState(robotName, timestamp);
 
         if (robotState)
         {
-            const auto globalPose = queryGlobalPose(description, timestamp);
+            const auto globalPose = queryGlobalPose(robotName, timestamp);
             if (not globalPose)
             {
-                ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
+                ARMARX_VERBOSE << "Failed to query global pose for robot " << robotName;
                 return std::nullopt;
             }
             robotState->globalPose = *globalPose;
@@ -203,14 +203,14 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<RobotState>
-    RobotReader::queryJointState(const description::RobotDescription& description,
+    RobotReader::queryJointState(const std::string& robotName,
                                  const armem::Time& timestamp) const
     {
-        const auto proprioception = queryProprioception(description, timestamp);
+        const auto proprioception = queryProprioception(robotName, timestamp);
 
         if (not proprioception.has_value())
         {
-            ARMARX_VERBOSE << "Failed to query proprioception for robot '" << description.name
+            ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName
                            << "'.";
             return std::nullopt;
         }
@@ -223,14 +223,14 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<::armarx::armem::robot_state::localization::Transform>
-    RobotReader::queryOdometryPose(const description::RobotDescription& description,
+    RobotReader::queryOdometryPose(const std::string& robotName,
                                    const armem::Time& timestamp) const
     {
 
         robot_state::localization::TransformQuery query{
             .header = {.parentFrame = OdometryFrame,
                        .frame = constants::robotRootNodeName,
-                       .agent = description.name,
+                       .agent = robotName,
                        .timestamp = timestamp}};
 
         // try
@@ -250,7 +250,7 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<armarx::armem::arondto::Proprioception>
-    RobotReader::queryProprioception(const description::RobotDescription& description,
+    RobotReader::queryProprioception(const std::string& robotName,
                                      const armem::Time& timestamp) const // Why timestamp?!?!
     {
         // TODO(fabian.reister): how to deal with multiple providers?
@@ -258,12 +258,12 @@ namespace armarx::armem::robot_state::client::common
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
-        ARMARX_DEBUG << "Querying robot description for robot: " << description;
+        ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
 
         // clang-format off
         qb
         .coreSegments().withName(constants::proprioceptionCoreSegment)
-        .providerSegments().withName(description.name) // agent
+        .providerSegments().withName(robotName) // agent
         .entities().all() // TODO
         .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
@@ -281,7 +281,7 @@ namespace armarx::armem::robot_state::client::common
                 return std::nullopt;
             }
 
-            return getRobotProprioception(qResult.memory, description.name);
+            return getRobotProprioception(qResult.memory, robotName);
         }
         catch (...)
         {
@@ -292,19 +292,19 @@ namespace armarx::armem::robot_state::client::common
     }
 
     RobotReader::JointTrajectory
-    RobotReader::queryJointStates(const description::RobotDescription& description,
+    RobotReader::queryJointStates(const std::string& robotName,
                                   const armem::Time& begin,
                                   const armem::Time& end) const
     {
         armem::client::query::Builder qb;
 
-        ARMARX_DEBUG << "Querying robot joint states for robot: `" << description
+        ARMARX_DEBUG << "Querying robot joint states for robot: `" << robotName
                      << "` on time interval [" << begin << "," << end << "]";
 
         // clang-format off
         qb
         .coreSegments().withName(constants::proprioceptionCoreSegment)
-        .providerSegments().withName(description.name) // agent
+        .providerSegments().withName(robotName) // agent
         .entities().all() // TODO
         .snapshots().timeRange(begin, end);
         // clang-format on
@@ -322,7 +322,7 @@ namespace armarx::armem::robot_state::client::common
                 return {};
             }
 
-            return getRobotJointStates(qResult.memory, description.name);
+            return getRobotJointStates(qResult.memory, robotName);
         }
         catch (...)
         {
@@ -333,7 +333,7 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<PlatformState>
-    RobotReader::queryPlatformState(const description::RobotDescription& description,
+    RobotReader::queryPlatformState(const std::string& robotName,
                                     const armem::Time& timestamp) const
     {
         // TODO(fabian.reister): how to deal with multiple providers?
@@ -341,12 +341,12 @@ namespace armarx::armem::robot_state::client::common
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
-        ARMARX_DEBUG << "Querying robot description for robot: " << description;
+        ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
 
         // clang-format off
         qb
         .coreSegments().withName(constants::proprioceptionCoreSegment)
-        .providerSegments().withName(description.name) // agent
+        .providerSegments().withName(robotName) // agent
         .entities().all() // TODO
         .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
@@ -364,7 +364,7 @@ namespace armarx::armem::robot_state::client::common
                 return std::nullopt;
             }
 
-            return getRobotPlatformState(qResult.memory, description.name);
+            return getRobotPlatformState(qResult.memory, robotName);
         }
         catch (...)
         {
@@ -375,13 +375,13 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<RobotState::Pose>
-    RobotReader::queryGlobalPose(const description::RobotDescription& description,
+    RobotReader::queryGlobalPose(const std::string& robotName,
                                  const armem::Time& timestamp) const
     {
         try
         {
             const auto result = transformReader.getGlobalPose(
-                description.name, constants::robotRootNodeName, timestamp);
+                robotName, constants::robotRootNodeName, timestamp);
             if (not result)
             {
                 return std::nullopt;
@@ -521,19 +521,19 @@ namespace armarx::armem::robot_state::client::common
 
     // force torque for left and right
     std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
-    RobotReader::queryForceTorque(const description::RobotDescription& description,
+    RobotReader::queryForceTorque(const std::string& robotName,
                                   const armem::Time& timestamp) const
     {
 
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
-        ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+        ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
 
         // clang-format off
         qb
         .coreSegments().withName(constants::proprioceptionCoreSegment)
-        .providerSegments().withName(description.name) // agent
+        .providerSegments().withName(robotName) // agent
         .entities().all() // TODO
         .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
@@ -551,7 +551,7 @@ namespace armarx::armem::robot_state::client::common
                 return std::nullopt;
             }
 
-            return getForceTorque(qResult.memory, description.name);
+            return getForceTorque(qResult.memory, robotName);
         }
         catch (...)
         {
@@ -563,7 +563,7 @@ namespace armarx::armem::robot_state::client::common
 
     // force torque for left and right
     std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
-    RobotReader::queryForceTorques(const description::RobotDescription& description,
+    RobotReader::queryForceTorques(const std::string& robotName,
                                    const armem::Time& start,
                                    const armem::Time& end) const
     {
@@ -571,12 +571,12 @@ namespace armarx::armem::robot_state::client::common
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
-        ARMARX_DEBUG << "Querying force torques description for robot: " << description;
+        ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
 
         // clang-format off
         qb
         .coreSegments().withName(constants::proprioceptionCoreSegment)
-        .providerSegments().withName(description.name) // agent
+        .providerSegments().withName(robotName) // agent
         .entities().all() // TODO
         .snapshots().timeRange(start, end);
         // clang-format on
@@ -594,7 +594,7 @@ namespace armarx::armem::robot_state::client::common
                 return std::nullopt;
             }
 
-            return getForceTorques(qResult.memory, description.name);
+            return getForceTorques(qResult.memory, robotName);
         }
         catch (...)
         {
@@ -605,18 +605,18 @@ namespace armarx::armem::robot_state::client::common
     }
 
     std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
-    RobotReader::queryToF(const description::RobotDescription& description,
+    RobotReader::queryToF(const std::string& robotName,
                           const armem::Time& timestamp) const
     {
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
-        ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+        ARMARX_DEBUG << "Querying ToF data for robot: " << robotName;
 
         // clang-format off
         qb
         .coreSegments().withName(constants::exteroceptionCoreSegment)
-        .providerSegments().withName(description.name) // agent
+        .providerSegments().withName(robotName) // agent
         .entities().all() // TODO
         .snapshots().beforeOrAtTime(timestamp);
         // clang-format on
@@ -634,7 +634,7 @@ namespace armarx::armem::robot_state::client::common
                 return std::nullopt;
             }
 
-            return getToF(qResult.memory, description.name);
+            return getToF(qResult.memory, robotName);
         }
         catch (...)
         {

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h

@@ -64,29 +64,26 @@ namespace armarx::armem::robot_state::client::common
         std::vector<description::RobotDescription>
         queryDescriptions(const armem::Time& timestamp) const;
 
-        std::optional<RobotState> queryState(const description::RobotDescription& description,
+        std::optional<RobotState> queryState(const std::string& robotName,
                                              const armem::Time& timestamp) const;
 
-        std::optional<RobotState> queryJointState(const description::RobotDescription& description,
+        std::optional<RobotState> queryJointState(const std::string& robotName,
                                                   const armem::Time& timestamp) const;
 
         std::optional<armarx::armem::arondto::Proprioception>
-        queryProprioception(const description::RobotDescription& description,
-                            const armem::Time& timestamp) const;
+        queryProprioception(const std::string& robotName, const armem::Time& timestamp) const;
 
         using JointTrajectory = std::map<armem::Time, RobotState::JointMap>;
 
-        JointTrajectory queryJointStates(const description::RobotDescription& description,
+        JointTrajectory queryJointStates(const std::string& robotName,
                                          const armem::Time& begin,
                                          const armem::Time& end) const;
 
-        std::optional<RobotState::Pose>
-        queryGlobalPose(const description::RobotDescription& description,
-                        const armem::Time& timestamp) const;
+        std::optional<RobotState::Pose> queryGlobalPose(const std::string& robotName,
+                                                        const armem::Time& timestamp) const;
 
-        std::optional<PlatformState>
-        queryPlatformState(const description::RobotDescription& description,
-                           const armem::Time& timestamp) const;
+        std::optional<PlatformState> queryPlatformState(const std::string& robotName,
+                                                        const armem::Time& timestamp) const;
 
         void setSyncTimeout(const armem::Duration& duration);
         void setSleepAfterSyncFailure(const armem::Duration& duration);
@@ -98,19 +95,17 @@ namespace armarx::armem::robot_state::client::common
         };
 
         std::optional<std::map<Hand, proprioception::ForceTorque>>
-        queryForceTorque(const description::RobotDescription& description,
-                         const armem::Time& timestamp) const;
+        queryForceTorque(const std::string& robotName, const armem::Time& timestamp) const;
 
         std::optional<
             std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
-        queryForceTorques(const description::RobotDescription& description,
+        queryForceTorques(const std::string& robotName,
                           const armem::Time& start,
                           const armem::Time& end) const;
 
 
         std::optional<std::map<Hand, exteroception::ToF>>
-        queryToF(const description::RobotDescription& description,
-                 const armem::Time& timestamp) const;
+        queryToF(const std::string& robotName, const armem::Time& timestamp) const;
 
         /**
          * @brief retrieve the robot's pose in the odometry frame. 
@@ -122,8 +117,7 @@ namespace armarx::armem::robot_state::client::common
          * @return std::optional<RobotState::Pose> 
          */
         std::optional<::armarx::armem::robot_state::localization::Transform>
-        queryOdometryPose(const description::RobotDescription& description,
-                          const armem::Time& timestamp) const;
+        queryOdometryPose(const std::string& robotName, const armem::Time& timestamp) const;
 
     protected:
         // by default, no timeout mechanism
@@ -174,7 +168,6 @@ namespace armarx::armem::robot_state::client::common
 
 } // namespace armarx::armem::robot_state::client::common
 
-
 namespace armarx::armem::robot_state
 {
     using client::common::RobotReader;

source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp

@@ -15,24 +15,12 @@
 
 namespace armarx::armem::robot_state::client::common
 {
-    // TODO(fabian.reister): register property defs
-    void
-    VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
-    {
-        RobotReader::registerPropertyDefinitions(def);
-    }
 
     bool
     VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
                                          const armem::Time& timestamp) const
     {
-        // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
-        // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
-
-        const description::RobotDescription robotDescription{.name = robot.getName(),
-                                                             .xml = PackagePath{"", ""}};
-
-        const auto robotState = queryState(robotDescription, timestamp);
+        const auto robotState = queryState(robot.getName(), timestamp);
         if (not robotState)
         {
             ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
@@ -51,13 +39,7 @@ namespace armarx::armem::robot_state::client::common
     VirtualRobotReader::synchronizeRobotJoints(VirtualRobot::Robot& robot,
                                                const armem::Time& timestamp) const
     {
-        // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
-        // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
-
-        const description::RobotDescription robotDescription{.name = robot.getName(),
-                                                             .xml = PackagePath{"", ""}};
-
-        const auto robotState = queryJointState(robotDescription, timestamp);
+        const auto robotState = queryJointState(robot.getName(), timestamp);
         if (not robotState)
         {
             ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
@@ -145,16 +127,5 @@ namespace armarx::armem::robot_state::client::common
         return nullptr;
     }
 
-    std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
-    VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
-    {
-        const auto description = queryDescription(name, timestamp);
-        if (not description.has_value())
-        {
-            return std::nullopt;
-        }
-        return RobotReader::queryForceTorque(description.value(), timestamp);
-    }
-
 
 } // namespace armarx::armem::robot_state::client::common

source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h

@@ -44,8 +44,6 @@ namespace armarx::armem::robot_state::client::common
 
         ~VirtualRobotReader() override = default;
 
-        void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
-
         [[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
                                             const armem::Time& timestamp) const;
 
@@ -71,10 +69,6 @@ namespace armarx::armem::robot_state::client::common
                                  VirtualRobot::RobotIO::RobotDescription::eStructure,
                              bool blocking = true);
 
-        std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
-        queryForceTorque(const std::string& name, const armem::Time& timestamp);
-        using RobotReader::queryForceTorque;
-
     private:
         [[nodiscard]] VirtualRobot::RobotPtr
         _getSynchronizedRobot(const std::string& name,