Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed camera based approach

Browse Source
This commit was merged in pull request #3.
This commit is contained in:
Pimpest
2026-04-18 04:32:02 +02:00
parent a1c39cb943
commit be9744ac47
2 changed files with 11 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
toid_behaviors/include/toid_behaviors/rolling_average.hpp
View File

@@ -21,7 +21,7 @@ public:
if(size_ == 0) {
return {};
}
if (size_-1 == data_count_) {
if (size_ == data_count_) {
accum_x_ -= poses_[front_idx_].position.x;
accum_y_ -= poses_[front_idx_].position.y;
accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation);
@@ -30,16 +30,16 @@ public:
data_count_--;
}
back_idx_ += 1;
back_idx_ %= size_;
data_count_++;
poses_[back_idx_] = pose;
accum_x_ += poses_[back_idx_].position.x;
accum_y_ += poses_[back_idx_].position.y;
accum_theta_ += tf2::getYaw(poses_[back_idx_].orientation);
back_idx_ += 1;
back_idx_ %= size_;
data_count_++;
return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_};
}