From 810449b1d53e12373718ceca5f56bdbdd3d3943e Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Fri, 12 Aug 2022 17:39:55 +0200
Subject: [PATCH] Add comparison operator overload for FramedPose
---
source/RobotAPI/libraries/core/FramedPose.cpp | 11 +++++++++++
source/RobotAPI/libraries/core/FramedPose.h | 2 ++
2 files changed, 13 insertions(+)
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 21049a274..e309cc113 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -1220,5 +1220,16 @@ namespace armarx
return stream;
}
+ bool operator==(const FramedPose& pose1, const FramedPose& pose2)
+ {
+ if (pose1.frame != pose2.frame || pose1.agent != pose2.agent) return false;
+ return (pose1.toEigen().isApprox(pose2.toEigen()));
+ }
+
+ bool operator!=(const FramedPose& pose1, const FramedPose& pose2)
+ {
+ return !(pose1 == pose2);
+ }
+
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 01f5c9c6a..0abe9ab2a 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -311,6 +311,8 @@ namespace armarx
FramedOrientationPtr getOrientation() const;
static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation);
+ friend bool operator==(const FramedPose& pose1, const FramedPose& pose2);
+ friend bool operator!=(const FramedPose& pose1, const FramedPose& pose2);
public:
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
--
GitLab