From 09443d9cc36e938069fee354b09787e6f65559c2 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 5 Aug 2021 12:01:50 +0200
Subject: [PATCH] Switch from boost circular buffer to std deque
---
include/qrw/Estimator.hpp | 4 ++--
src/Estimator.cpp | 15 +++++++++------
2 files changed, 11 insertions(+), 8 deletions(-)
diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index aba8f7cd..cb5eea4d 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -19,7 +19,7 @@
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
-#include <boost/circular_buffer.hpp>
+#include <deque>
class ComplementaryFilter
{
@@ -281,7 +281,7 @@ private:
int N_queue_;
Vector3 v_filt_bis_;
Vector3 h_v_bis_;
- boost::circular_buffer<double> vx_queue_, vy_queue_, vz_queue_;
+ std::deque<double> vx_queue_, vy_queue_, vz_queue_;
};
#endif // ESTIMATOR_H_INCLUDED
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index e934873b..17b5db83 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -100,9 +100,9 @@ void Estimator::initialize(Params& params)
alpha_v_ = -y + std::sqrt(y * y + 2 * y);
N_queue_ = static_cast<int>(std::round(1.0/(dt_wbc * 3.0)));
- vx_queue_.resize(N_queue_, 0.0); // Buffer full of 0.0
- vy_queue_.resize(N_queue_, 0.0); // Buffer full of 0.0
- vz_queue_.resize(N_queue_, 0.0); // Buffer full of 0.0
+ vx_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ vy_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ vz_queue_.resize(N_queue_, 0.0); // List full of 0.0
// Filtering velocities used for security checks
fc = 6.0; // Cut frequency
@@ -376,9 +376,12 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
v_filt_.block(3, 0, 3, 1) = filt_ang_vel; // Angular velocities are already directly from PyBullet
v_filt_.tail(12) = actuators_vel_; // Actuators velocities are already directly from PyBullet
- vx_queue_.push_back(b_filt_lin_vel_(0));
- vy_queue_.push_back(b_filt_lin_vel_(1));
- vz_queue_.push_back(b_filt_lin_vel_(2));
+ vx_queue_.pop_back();
+ vy_queue_.pop_back();
+ vz_queue_.pop_back();
+ vx_queue_.push_front(b_filt_lin_vel_(0));
+ vy_queue_.push_front(b_filt_lin_vel_(1));
+ vz_queue_.push_front(b_filt_lin_vel_(2));
v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
--
GitLab