Getter and setter for localizer (#65)

* Getter and setter for localizer

* Committing clang-format changes

---------

Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com>

include/VOSS/localizer/ADILocalizer.hpp

@@ -33,10 +33,11 @@ class ADILocalizer : public AbstractLocalizer {
     double get_middle_encoder_value();
     double get_imu_value();
 
-    void update();
-    void calibrate();
+    void update() override;
+    void calibrate() override;
 
-    void set_pose(Pose pose);
+    void set_pose(Pose pose) override;
+    void set_pose(double x, double y, double theta) override;
 };
 
 } // namespace voss::localizer

include/VOSS/localizer/AbstractLocalizer.hpp

@@ -18,9 +18,12 @@ class AbstractLocalizer {
     virtual void update() = 0;
     void begin_localization();
 
-    void set_pose(Pose pose);
+    virtual void set_pose(Pose pose);
+    virtual void set_pose(double x, double y, double theta);
 
     Pose get_pose();
+    double get_x();
+    double get_y();
     double get_orientation_rad();
     double get_orientation_deg();
     Point get_position();

include/VOSS/localizer/IMELocalizer.hpp

@@ -36,10 +36,11 @@ class IMELocalizer : public AbstractLocalizer {
     double get_middle_encoder_value();
     double get_imu_value();
 
-    void update();
-    void calibrate();
+    void update() override;
+    void calibrate() override;
 
-    void set_pose(Pose pose);
+    void set_pose(Pose pose) override;
+    void set_pose(double x, double y, double theta) override;
 };
 
 } // namespace voss::localizer

include/VOSS/localizer/TrackingWheelLocalizer.hpp

@@ -26,7 +26,8 @@ class TrackingWheelLocalizer : public AbstractLocalizer {
                            double left_right_dist, double middle_dist);
     void update() override;
     void calibrate() override;
-    void set_pose(Pose pose);
+    void set_pose(Pose pose) override;
+    void set_pose(double x, double y, double theta) override;
 
     friend class TrackingWheelLocalizerBuilder;
 };

src/VOSS/localizer/ADILocalizer.cpp

@@ -153,5 +153,8 @@ void ADILocalizer::set_pose(Pose pose) {
         this->imu->set_rotation(-pose.theta.value());
     }
 }
+void ADILocalizer::set_pose(double x, double y, double theta) {
+    this->set_pose({x, y, theta});
+}
 
 } // namespace voss::localizer

src/VOSS/localizer/AbstractLocalizer.cpp

@@ -26,7 +26,7 @@ void AbstractLocalizer::begin_localization() {
     });
 }
 // These last few functions allows the user to set and get values of the robot's
-// pose while keeing the values safe with mutex
+// pose while keeping the values safe with mutex
 void AbstractLocalizer::set_pose(Pose pose) {
     std::unique_lock<pros::Mutex> lock(this->mtx);
     if (pose.theta.has_value()) {
@@ -38,6 +38,10 @@ void AbstractLocalizer::set_pose(Pose pose) {
     }
 }
 
+void AbstractLocalizer::set_pose(double x, double y, double theta) {
+    this->set_pose({x, y, theta});
+}
+
 Pose AbstractLocalizer::get_pose() {
     std::unique_lock<pros::Mutex> lock(this->mtx);
     Pose ret = {this->pose.x.load(), this->pose.y.load(),
@@ -46,6 +50,18 @@ Pose AbstractLocalizer::get_pose() {
     return ret;
 }
 
+double AbstractLocalizer::get_x() {
+    std::unique_lock<pros::Mutex> lock(this->mtx);
+    double ret = this->pose.x;
+    return ret;
+}
+
+double AbstractLocalizer::get_y() {
+    std::unique_lock<pros::Mutex> lock(this->mtx);
+    double ret = this->pose.y;
+    return ret;
+}
+
 double AbstractLocalizer::get_orientation_rad() {
     std::unique_lock<pros::Mutex> lock(this->mtx);
     double ret = this->pose.theta;

src/VOSS/localizer/IMELocalizer.cpp

@@ -158,5 +158,8 @@ void IMELocalizer::set_pose(Pose pose) {
         this->imu->set_rotation(-pose.theta.value());
     }
 }
+void IMELocalizer::set_pose(double x, double y, double theta) {
+    this->set_pose({x, y, theta});
+}
 
 } // namespace voss::localizer

src/VOSS/localizer/TrackingWheelLocalizer.cpp

@@ -92,4 +92,8 @@ void TrackingWheelLocalizer::set_pose(Pose pose) {
     }
 }
 
+void TrackingWheelLocalizer::set_pose(double x, double y, double theta) {
+    this->set_pose({x, y, theta});
+}
+
 } // namespace voss::localizer