Switch from boost circular buffer to std deque

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

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_;