From 4320b7c11ee4d3bd88cc6490904a98a1d55c989f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 14 May 2021 16:38:32 +0200
Subject: [PATCH] cleanup
---
.../client/common/RobotReader.h | 45 ++++++++++++-------
.../client/common/VirtualRobotReader.h | 10 +++--
.../client/localization/TransformReader.cpp | 9 +---
.../client/localization/TransformReader.h | 2 +-
4 files changed, 39 insertions(+), 27 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index ba4d3d417..b5bd01e5d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 91e5392d2..5958bbca1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index ea636eeb1..3a4fcd85f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 3d7bf59ee..2d1d98ddf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/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.
*
--
GitLab