Add ==, !=, and << operators

include/lemlib/pose.hpp

@@ -11,6 +11,7 @@
 #pragma once
 
 #include <string>
+#include <ostream>
 
 namespace lemlib {
 class Pose {
@@ -123,6 +124,14 @@ class Pose {
         Pose rotate(float angle) const;
 };
 
+/**
+ * @brief output and format pose into a stream
+ *
+ * @param pose
+ * @return std::ostream
+ */
+std::ostream& operator<<(std::ostream& os, const lemlib::Pose& pose);
+
 /**
  * @brief Format a pose
  *

src/lemlib/pose.cpp

@@ -145,8 +145,30 @@ Pose Pose::rotate(float angle) const {
     return Pose(x * cosAngle - y * sinAngle, x * sinAngle + y * cosAngle, theta);
 }
 
-std::string format_as(const Pose& pose) {
+/**
+ * @brief Check that x, y, and theta are equal between two poses
+ *
+ * @param other the other pose
+ * @return bool
+ */
+bool lemlib::Pose::operator==(const lemlib::Pose& other) const {
+    return this->x == other.x && this->y == other.y && this->theta == other.theta;
+}
+
+/**
+ * @brief Check that x, y, and theta are NOT equal between two poses
+ *
+ * @param other the other pose
+ * @return bool
+ */
+bool lemlib::Pose::operator!=(const lemlib::Pose& other) const { return !(*this == other); }
+
+std::ostream& lemlib::operator<<(std::ostream& os, const lemlib::Pose& pose) {
+    return os << lemlib::format_as(pose).c_str();
+}
+
+std::string lemlib::format_as(const lemlib::Pose& pose) {
     // the double brackets become single brackets
     return fmt::format("lemlib::Pose {{ x: {}, y: {}, theta: {} }}", pose.x, pose.y, pose.theta);
 }
-} // namespace lemlib
+} // namespace lemlib

test/pose.cpp

@@ -8,6 +8,7 @@ TEST(PoseTest, BasicConstructor) {
     EXPECT_EQ(pose.y, 2);
     EXPECT_EQ(pose.theta, 0);
 }
+
 TEST(PoseTest, ConstructorWithTheta) {
     lemlib::Pose pose {1, 2, 30};
 
@@ -16,64 +17,72 @@ TEST(PoseTest, ConstructorWithTheta) {
     EXPECT_EQ(pose.theta, 30);
 }
 
+TEST(PoseTest, EqualityOperator) {
+    lemlib::Pose pose1 {1, 2};
+    lemlib::Pose pose2 {1, 2};
+    lemlib::Pose pose3 {-3, 5};
+
+    EXPECT_EQ(pose1, pose2);
+    EXPECT_NE(pose1, pose3);
+    EXPECT_NE(pose2, pose3);
+}
+
 TEST(PoseTest, CopyConstructor) {
-  lemlib::Pose startPose {1, 2, 30};
-  lemlib::Pose pose(startPose);
+    lemlib::Pose startPose {1, 2, 30};
+    lemlib::Pose pose(startPose);
 
-  EXPECT_EQ(pose.x, 1);
-  EXPECT_EQ(pose.y, 2);
-  EXPECT_EQ(pose.theta, 30);
+    EXPECT_EQ(startPose, pose);
 }
 
 TEST(PoseTest, Addition) {
-  lemlib::Pose pose1 {-8, 2, 90};
-  lemlib::Pose pose2 {9, -4, 30};
+    lemlib::Pose pose1 {-8, 2, 90};
+    lemlib::Pose pose2 {9, -4, 30};
 
-  lemlib::Pose sum = pose1+pose2;
+    lemlib::Pose sum = pose1 + pose2;
 
-  EXPECT_EQ(sum.x, 1);
-  EXPECT_EQ(sum.y, -2);
-  EXPECT_EQ(sum.theta, 90);
+    EXPECT_EQ(sum.x, 1);
+    EXPECT_EQ(sum.y, -2);
+    EXPECT_EQ(sum.theta, 90);
 }
 
 TEST(PoseTest, Subtraction) {
-  lemlib::Pose pose1 {-8, 2, 90};
-  lemlib::Pose pose2 {9, -4, 30};
+    lemlib::Pose pose1 {-8, 2, 90};
+    lemlib::Pose pose2 {9, -4, 30};
 
-  lemlib::Pose diff = pose1-pose2;
+    lemlib::Pose diff = pose1 - pose2;
 
-  EXPECT_EQ(diff.x, -17);
-  EXPECT_EQ(diff.y, 6);
-  EXPECT_EQ(diff.theta, 90);
+    EXPECT_EQ(diff.x, -17);
+    EXPECT_EQ(diff.y, 6);
+    EXPECT_EQ(diff.theta, 90);
 }
 
 TEST(PoseTest, ScalarMultiplication) {
-  lemlib::Pose pose1 {-8, 2, 90};
-  float scalar = 2;
+    lemlib::Pose pose1 {-8, 2, 90};
+    float scalar = 2;
 
-  lemlib::Pose product = pose1*scalar;
+    lemlib::Pose product = pose1 * scalar;
 
-  EXPECT_EQ(product.x, -16);
-  EXPECT_EQ(product.y, 4);
-  EXPECT_EQ(product.theta, 90);
+    EXPECT_EQ(product.x, -16);
+    EXPECT_EQ(product.y, 4);
+    EXPECT_EQ(product.theta, 90);
 }
 
 TEST(PoseTest, ScalarDivision) {
-  lemlib::Pose pose1 {-8, 2, 90};
-  float scalar = 2;
+    lemlib::Pose pose1 {-8, 2, 90};
+    float scalar = 2;
 
-  lemlib::Pose quotient = pose1/scalar;
+    lemlib::Pose quotient = pose1 / scalar;
 
-  EXPECT_EQ(quotient.x, -4);
-  EXPECT_EQ(quotient.y, 1);
-  EXPECT_EQ(quotient.theta, 90);
+    EXPECT_EQ(quotient.x, -4);
+    EXPECT_EQ(quotient.y, 1);
+    EXPECT_EQ(quotient.theta, 90);
 }
 
 TEST(PoseTest, MatrixMultiplication) {
-  lemlib::Pose pose1 {-8, 2, 90};
-  lemlib::Pose pose2 {9, -4, 30};
+    lemlib::Pose pose1 {-8, 2, 90};
+    lemlib::Pose pose2 {9, -4, 30};
 
-  float product = pose1*pose2;
+    float product = pose1 * pose2;
 
-  EXPECT_EQ(product,-8*9 + 2*-4);
+    EXPECT_EQ(product, -8 * 9 + 2 * -4);
 }