67 lines
1.4 KiB
C++
67 lines
1.4 KiB
C++
#pragma once
|
|
|
|
#include <algorithm>
|
|
#include <vector>
|
|
|
|
#include "geometry_msgs/msg/pose.hpp"
|
|
#include "tf2/utils.hpp"
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
class RollingAverage
|
|
{
|
|
public:
|
|
using Pose2D = std::tuple<double, double, double>;
|
|
RollingAverage(size_t size = 0) : poses_(size), size_(size) {}
|
|
|
|
Pose2D push(geometry_msgs::msg::Pose & pose)
|
|
{
|
|
if(size_ == 0) {
|
|
return {};
|
|
}
|
|
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);
|
|
front_idx_ += 1;
|
|
front_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_};
|
|
}
|
|
|
|
void reset() {
|
|
data_count_ = 0;
|
|
front_idx_ = 0;
|
|
back_idx_ = 0;
|
|
accum_x_ = 0;
|
|
accum_y_ = 0;
|
|
accum_theta_ = 0;
|
|
}
|
|
|
|
private:
|
|
std::vector<geometry_msgs::msg::Pose> poses_;
|
|
const size_t size_;
|
|
size_t front_idx_ = 0;
|
|
size_t back_idx_ = 0;
|
|
size_t data_count_ = 0;
|
|
|
|
double accum_x_ = 0;
|
|
double accum_y_ = 0;
|
|
double accum_theta_ = 0;
|
|
};
|
|
|
|
} // namespace toid
|