Commit 4b946ae6 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Add Simple Moving Average to detect a collision

parent 495704ae
......@@ -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
......
......@@ -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
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
......@@ -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]
......
......@@ -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))
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment