Add Simple Moving Average to detect a collision

README.md

@@ -21,6 +21,8 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 - ~**collision_acc_th** (Double; default: 9.8; min: 0.1; max: 160.0) Threshold to detect a collision.
 - ~**collision_transition_counter_en** (Boolean; default: False) Enable the collision counter. 
 - ~**collision_counter_limit** (Integer; default: 6; min: 1; max: 30) Number of low acc imu meassures to detect an end of collision.
+- ~**sma_en** (Boolean; default: False) Enable Simple Moving average.
+- ~**sma_window** (Integer; default: 10; min: 1; max: 100) Number of data points for the SMA.
 
 ## Installation
 

cfg/CollisionManager.cfg

@@ -45,6 +45,8 @@ gen.add("fixed_frame",                          str_t,      0,           "Fixed
 gen.add("tf_timeout",                           double_t,   0,           "Timeout to find a transform",                           0.2, 0.1, 2.0)
 gen.add("collision_transition_counter_en",      bool_t,     0,           "Enable the collision counter",                          False)
 gen.add("collision_counter_limit",              int_t,      0,           "Number of low acc imu meassures to detect an end of collision",    6, 1, 30)
+gen.add("sma_en",                               bool_t,     0,           "Enable Simple Moving average",                          False)
+gen.add("sma_window",                           int_t,      0,           "Number of data points for the SMA",                     10, 1, 100)
 
 
 exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file

config/params.yaml

 rate: 240
 tf_timeout: 0.2
-collision_acc_th: 5.0
+collision_acc_th: 3.0
 collision_transition_counter_en: True
 collision_counter_limit: 12
 err_msg_rate: 0.5
+sma_en: True
+sma_window: 4
 fixed_frame: imu_bno055

include/collision_manager_alg_node.h

@@ -54,6 +54,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     ros::Time collision_start_stamp_; ///< Time stamp of a collision initial detection.
     Eigen::Vector3d max_acc_; ///< The linear accelerations meassured from fized frame reference at maximum acceleration module.
     int low_acc_count_; ///< Low acceleration meassures counter to detect the end of a collision.
+    std::list<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > global_acc_data_window_;///<
 
     // [publisher attributes]
 

src/collision_manager_alg_node.cpp

@@ -44,6 +44,7 @@ CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
 {
   // [free dynamic memory]
   pthread_mutex_destroy(&this->imu_mutex_);
+  std::list<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> >().swap(this->global_acc_data_window_);
 }
 
 void CollisionManagerAlgNode::mainNodeThread(void)
@@ -58,7 +59,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
     Eigen::Vector3d global_acc;
     if (get_global_acc(global_acc))
     {
-      double acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
+      double acc_norm_2d;
+      if (this->config_.sma_en)
+      {
+        if (this->global_acc_data_window_.size() >= this->config_.sma_window)
+          this->global_acc_data_window_.pop_front();
+        this->global_acc_data_window_.push_back(global_acc);
+        double sma_acc_x = 0.0, sma_acc_y = 0.0;
+        for (auto it = this->global_acc_data_window_.begin(); it != this->global_acc_data_window_.end(); ++it)
+        {
+          Eigen::Vector3d a = *it;
+          sma_acc_x += a(0);
+          sma_acc_y += a(1);
+        }
+        sma_acc_x /= (double) this->global_acc_data_window_.size();
+        sma_acc_y /= (double) this->global_acc_data_window_.size();
+        acc_norm_2d = std::sqrt(std::pow(sma_acc_x,2) + std::pow(sma_acc_y,2));
+        // ROS_INFO_STREAM("acc_norm_2d " << acc_norm_2d << "; window size " << this->global_acc_data_window_.size());
+      }
+      else
+        acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
       if (this->in_collision_)
       {
         double max_acc_norm_2d = std::sqrt(std::pow(this->max_acc_(0),2) + std::pow(this->max_acc_(1),2));
@@ -74,8 +94,16 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           this->in_collision_ = false;
           this->low_acc_count_ = 0;
         }
-        else if (acc_norm_2d > max_acc_norm_2d)
-          this->max_acc_ = global_acc;
+        else
+        {
+          double acc;
+          if (this->config_.sma_en)
+            acc = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2)); 
+          else 
+            acc = acc_norm_2d;
+          if (acc > max_acc_norm_2d)
+            this->max_acc_ = global_acc;
+        } 
       }
       else if (check_start_of_collision(acc_norm_2d))
       {