diff --git a/include/VOSS/localizer/ADILocalizer.hpp b/include/VOSS/localizer/ADILocalizer.hpp index 5ca43c51..0a99c38e 100644 --- a/include/VOSS/localizer/ADILocalizer.hpp +++ b/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 \ No newline at end of file diff --git a/include/VOSS/localizer/AbstractLocalizer.hpp b/include/VOSS/localizer/AbstractLocalizer.hpp index 5c7d4dbe..0e2c3ed7 100644 --- a/include/VOSS/localizer/AbstractLocalizer.hpp +++ b/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(); diff --git a/include/VOSS/localizer/IMELocalizer.hpp b/include/VOSS/localizer/IMELocalizer.hpp index 379a6455..85bd43f3 100644 --- a/include/VOSS/localizer/IMELocalizer.hpp +++ b/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 \ No newline at end of file diff --git a/include/VOSS/localizer/TrackingWheelLocalizer.hpp b/include/VOSS/localizer/TrackingWheelLocalizer.hpp index 3207ef26..dba9bb31 100644 --- a/include/VOSS/localizer/TrackingWheelLocalizer.hpp +++ b/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; }; diff --git a/src/VOSS/localizer/ADILocalizer.cpp b/src/VOSS/localizer/ADILocalizer.cpp index 8835eb23..127d20f7 100644 --- a/src/VOSS/localizer/ADILocalizer.cpp +++ b/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 \ No newline at end of file diff --git a/src/VOSS/localizer/AbstractLocalizer.cpp b/src/VOSS/localizer/AbstractLocalizer.cpp index b7af9dc5..3037efbe 100644 --- a/src/VOSS/localizer/AbstractLocalizer.cpp +++ b/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 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 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 lock(this->mtx); + double ret = this->pose.x; + return ret; +} + +double AbstractLocalizer::get_y() { + std::unique_lock lock(this->mtx); + double ret = this->pose.y; + return ret; +} + double AbstractLocalizer::get_orientation_rad() { std::unique_lock lock(this->mtx); double ret = this->pose.theta; diff --git a/src/VOSS/localizer/IMELocalizer.cpp b/src/VOSS/localizer/IMELocalizer.cpp index 6995b62d..b2bd9113 100644 --- a/src/VOSS/localizer/IMELocalizer.cpp +++ b/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 \ No newline at end of file diff --git a/src/VOSS/localizer/TrackingWheelLocalizer.cpp b/src/VOSS/localizer/TrackingWheelLocalizer.cpp index 038758b6..94cb85cc 100644 --- a/src/VOSS/localizer/TrackingWheelLocalizer.cpp +++ b/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 \ No newline at end of file