cleanup

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

@@ -27,15 +27,20 @@
 #include "RobotAPI/libraries/armem/client.h"
 #include "RobotAPI/libraries/armem/client/Reader.h"
 
-#include "RobotAPI/libraries/armem_robot/types.h"
 #include "RobotAPI/libraries/armem_robot/client/interfaces.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
 
 #include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
 
 namespace armarx::armem::robot_state
 {
-    class RobotReader:
-        virtual public robot::ReaderInterface
+    /**
+     * @brief The RobotReader class.
+     *
+     * The purpose of this class is to synchronize the armem data structure armem::Robot
+     * with the memory.
+     */
+    class RobotReader : virtual public robot::ReaderInterface
     {
     public:
         RobotReader(armem::ClientReaderComponentPluginUser& component);
@@ -47,21 +52,32 @@ namespace armarx::armem::robot_state
 
         bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
 
-        std::optional<robot::Robot> get(const std::string& name, const armem::Time& timestamp) override;
-        robot::Robot get(const robot::RobotDescription& description, const armem::Time& timestamp) override;
+        std::optional<robot::Robot> get(const std::string& name,
+                                        const armem::Time& timestamp) override;
+        robot::Robot get(const robot::RobotDescription& description,
+                         const armem::Time& timestamp) override;
 
-        std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+        std::optional<robot::RobotDescription> queryDescription(const std::string& name,
+                const armem::Time& timestamp);
 
-        std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
-        std::optional<robot::RobotState::JointMap> queryJointState(const robot::RobotDescription& description, const armem::Time& timestamp) const;
-        std::optional<robot::RobotState::Pose> queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+        std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
+                const armem::Time& timestamp);
 
-    private:
+        std::optional<robot::RobotState::JointMap>
+        queryJointState(const robot::RobotDescription& description,
+                        const armem::Time& timestamp) const;
 
+        std::optional<robot::RobotState::Pose>
+        queryGlobalPose(const robot::RobotDescription& description,
+                        const armem::Time& timestamp) const;
 
-        std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-        std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-        std::optional<robot::RobotState::JointMap> getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+    private:
+        std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
+                const std::string& name) const;
+        std::optional<robot::RobotDescription>
+        getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+        std::optional<robot::RobotState::JointMap>
+        getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
         struct Properties
         {
@@ -81,5 +97,4 @@ namespace armarx::armem::robot_state
         client::robot_state::localization::TransformReader transformReader;
     };
 
-
-}  // namespace armarx::armem::robot_state
\ No newline at end of file
+} // namespace armarx::armem::robot_state
\ No newline at end of file

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

@@ -25,7 +25,6 @@
 
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/VirtualRobot.h>
-
 #include <VirtualRobot/XML/RobotIO.h>
 
 namespace armarx::armem::robot_state
@@ -36,6 +35,7 @@ namespace armarx::armem::robot_state
      * The aim of this class is to obtain a virtual robot instance and synchronize it
      * with the data (joint positions, global pose, ...) stored in the working memory.
      *
+     * This is only a lightweight wrapper of @see RobotReader for Simox's VirtualRobot class.
      */
     class VirtualRobotReader : virtual public RobotReader
     {
@@ -48,9 +48,11 @@ namespace armarx::armem::robot_state
 
         bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
 
-        VirtualRobot::RobotPtr getRobot(const std::string& name,
-                                        const armem::Time& timestamp,
-                                        const VirtualRobot::RobotIO::RobotDescription& loadMode = VirtualRobot::RobotIO::RobotDescription::eStructure);
+        VirtualRobot::RobotPtr
+        getRobot(const std::string& name,
+                 const armem::Time& timestamp,
+                 const VirtualRobot::RobotIO::RobotDescription& loadMode =
+                     VirtualRobot::RobotIO::RobotDescription::eStructure);
     };
 
 } // namespace armarx::armem::robot_state
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp

@@ -13,7 +13,6 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * @package    RobotAPI::ArmarXObjects::
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
@@ -69,7 +68,6 @@ namespace armarx::armem::client::robot_state::localization
     void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
     {
         ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
-        // MemoryConnector::registerPropertyDefinitions(def);
 
         const std::string prefix = propertyPrefix;
 
@@ -136,7 +134,7 @@ namespace armarx::armem::client::robot_state::localization
     {
         const auto& timestamp = query.header.timestamp;
 
-        const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
+        const auto durationEpsilon = IceUtil::Time::milliSeconds(-1);
 
         ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
 
@@ -148,12 +146,9 @@ namespace armarx::armem::client::robot_state::localization
         .coreSegments().withName(properties.localizationSegment)
         .providerSegments().withName(query.header.agent) // agent
         .entities().all() // parentFrame,frame
-        .snapshots().atTimeApprox(timestamp - durationEpsilon, IceUtil::Time::microSeconds(-1)); //timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
+        .snapshots().atTimeApprox(timestamp, durationEpsilon);
         // clang-format on
 
-        // TODO(fabian.reister): remove latest() and add atTime
-        // .atTime(timestamp); // transformation
-
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
         ARMARX_DEBUG << "Lookup result in reader: " << qResult;

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h

@@ -35,7 +35,7 @@ namespace armarx::armem::client::robot_state::localization
     * @ingroup RobotAPI-Components
     * A description of the component ExampleClient.
     *
-    * @class ExampleClient
+    * @class TransformReader
     * @ingroup Component-ExampleClient
     * @brief Brief description of class ExampleClient.
     *