diff --git a/include/VOSS/localizer/TrackingWheelLocalizer.hpp b/include/VOSS/localizer/TrackingWheelLocalizer.hpp index dba9bb31..76321a7a 100644 --- a/include/VOSS/localizer/TrackingWheelLocalizer.hpp +++ b/include/VOSS/localizer/TrackingWheelLocalizer.hpp @@ -12,11 +12,13 @@ class TrackingWheelLocalizer : public AbstractLocalizer { protected: std::atomic prev_left_pos, prev_right_pos, prev_middle_pos; AtomicPose prev_pose; + AtomicPose real_pose; std::atomic left_right_dist, middle_dist; std::unique_ptr left_tracking_wheel, right_tracking_wheel, middle_tracking_wheel; std::unique_ptr imu; + std::atomic offset; public: TrackingWheelLocalizer(std::unique_ptr left, @@ -28,6 +30,7 @@ class TrackingWheelLocalizer : public AbstractLocalizer { void calibrate() override; void set_pose(Pose pose) override; void set_pose(double x, double y, double theta) override; + void set_horizontal_offset(double offset); friend class TrackingWheelLocalizerBuilder; }; diff --git a/include/VOSS/localizer/TrackingWheelLocalizerBuilder.hpp b/include/VOSS/localizer/TrackingWheelLocalizerBuilder.hpp index 595b7277..ac099a49 100644 --- a/include/VOSS/localizer/TrackingWheelLocalizerBuilder.hpp +++ b/include/VOSS/localizer/TrackingWheelLocalizerBuilder.hpp @@ -8,6 +8,7 @@ class TrackingWheelLocalizerBuilder { private: double left_right_dist; double middle_dist; + double horizontal_offset; std::unique_ptr left_tracking_wheel; std::unique_ptr right_tracking_wheel; std::unique_ptr middle_tracking_wheel; @@ -38,6 +39,7 @@ class TrackingWheelLocalizerBuilder { TrackingWheelLocalizerBuilder& with_track_width(double track_width); TrackingWheelLocalizerBuilder& with_middle_dist(double middle_dist); TrackingWheelLocalizerBuilder& with_imu(int port); + TrackingWheelLocalizerBuilder& with_horizontal_offset(int offset); std::shared_ptr build(); }; diff --git a/src/VOSS/localizer/TrackingWheelLocalizer.cpp b/src/VOSS/localizer/TrackingWheelLocalizer.cpp index 1393a8f0..f98cf1fe 100644 --- a/src/VOSS/localizer/TrackingWheelLocalizer.cpp +++ b/src/VOSS/localizer/TrackingWheelLocalizer.cpp @@ -35,7 +35,7 @@ void TrackingWheelLocalizer::update() { } if (imu) { pose.theta = -to_radians(imu->get_rotation()); - delta_angle = pose.theta - prev_pose.theta; + delta_angle = real_pose.theta - prev_pose.theta; } else { delta_angle = (delta_right - delta_left) / (2 * left_right_dist); pose.theta += delta_angle; @@ -44,7 +44,7 @@ void TrackingWheelLocalizer::update() { prev_left_pos += delta_left; prev_right_pos += delta_right; prev_middle_pos += delta_middle; - prev_pose = pose; + prev_pose = real_pose; double local_x; double local_y; @@ -67,8 +67,10 @@ void TrackingWheelLocalizer::update() { double p = this->pose.theta - delta_angle / 2.0; // global angle // convert to absolute displacement - this->pose.x += cos(p) * local_x - sin(p) * local_y; - this->pose.y += sin(p) * local_x + cos(p) * local_y; + this->real_pose.x += cos(p) * local_x - sin(p) * local_y; + this->real_pose.y += sin(p) * local_x + cos(p) * local_y; + this->pose.x = real_pose.x + cos(pose.theta) * offset; + this->pose.y = real_pose.y + sin(pose.theta) * offset; } void TrackingWheelLocalizer::calibrate() { @@ -102,4 +104,9 @@ void TrackingWheelLocalizer::set_pose(double x, double y, double theta) { this->set_pose({x, y, theta}); } +void TrackingWheelLocalizer::set_horizontal_offset(double a) { + this->real_pose.x -= (a - this->offset) * cos(pose.theta); + this->real_pose.y -= (a - this->offset) * sin(pose.theta); + this->offset = a; +} } // namespace voss::localizer \ No newline at end of file diff --git a/src/VOSS/localizer/TrackingWheelLocalizerBuilder.cpp b/src/VOSS/localizer/TrackingWheelLocalizerBuilder.cpp index 59d20168..f9ee06fa 100644 --- a/src/VOSS/localizer/TrackingWheelLocalizerBuilder.cpp +++ b/src/VOSS/localizer/TrackingWheelLocalizerBuilder.cpp @@ -6,7 +6,7 @@ namespace voss::localizer { TrackingWheelLocalizerBuilder::TrackingWheelLocalizerBuilder() - : left_right_dist(0.0), middle_dist(0.0), left_tracking_wheel(nullptr), + : left_right_dist(0.0), middle_dist(0.0), horizontal_offset(0.0), left_tracking_wheel(nullptr), right_tracking_wheel(nullptr), middle_tracking_wheel(nullptr), imu(nullptr) { } @@ -127,11 +127,19 @@ TrackingWheelLocalizerBuilder::with_imu(int port) { return *this; } +TrackingWheelLocalizerBuilder& +TrackingWheelLocalizerBuilder::with_horizontal_offset(int offset) { + this->horizontal_offset = offset; + return *this; +} + std::shared_ptr TrackingWheelLocalizerBuilder::build() { - return std::make_shared( + auto result = std::make_shared( std::move(left_tracking_wheel), std::move(right_tracking_wheel), std::move(middle_tracking_wheel), std::move(imu), left_right_dist, middle_dist); + result->offset = this->horizontal_offset; + return result; } } // namespace voss::localizer \ No newline at end of file