diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index 507c498416663e779f03fcd26422eec648112124..8ec8aae4052b23b7d84cf0c0750c87563f3a5380 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -38,11 +38,13 @@ from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconf.level Description Default Min Max
-gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
-gen.add("collision_acc_th", double_t, 0, "Threshold to detect a collision", 9.8, 0.1, 40.0)
-gen.add("err_msg_rate", double_t, 0, "Rate to publish err messages", 0.5, 0.1, 1.0)
-gen.add("fixed_frame", str_t, 0, "Fixed frame", "map")
-gen.add("tf_timeout", double_t, 0, "Timeout to find a transform", 0.2, 0.1, 2.0)
+gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
+gen.add("collision_acc_th", double_t, 0, "Threshold to detect a collision", 9.8, 0.1, 40.0)
+gen.add("err_msg_rate", double_t, 0, "Rate to publish err messages", 0.5, 0.1, 1.0)
+gen.add("fixed_frame", str_t, 0, "Fixed frame", "map")
+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 to the collision counter", False)
+gen.add("collision_counter_limit", int_t, 0, "Number of low/high acc imu meassures to end/start a collision", 6, 1, 30)
exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file
diff --git a/config/params.yaml b/config/params.yaml
index 440b9a4469736b00a533533fef4f57aefec5591c..fd0cc6a68a8835af3a3610c31d4fc70bad9047a2 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,5 +1,7 @@
-rate: 100
+rate: 240
tf_timeout: 0.2
collision_acc_th: 5.0
+collision_transition_counter_en: True
+collision_counter_limit: 12
err_msg_rate: 0.5
fixed_frame: helena/base_link
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index a08940da94906d64ceb465e67608914908268dd5..c92ec90b43b63e213b39441311a039d9fe9f2fb5 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -45,11 +45,15 @@
class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>
{
private:
- Eigen::Vector3d imu_local_acc_; ///< Local input linear acceleration.
bool new_imu_input_; ///< Flag to know that new input data has been received.
+ Eigen::Vector3d imu_local_acc_; ///< Local input linear acceleration.
std::string imu_frame_; ///< IMU frame id.
- Eigen::Transform<double,3,Eigen::Affine> tf_fixed_frame_imu_; ///< Transform from fixed frame to IMU frame.
bool tf_loaded_; ///< Flag to know that tf_fixed_frame_imu has been loaded before.
+ Eigen::Transform<double,3,Eigen::Affine> tf_fixed_frame_imu_; ///< Transform from fixed frame to IMU frame.
+ bool in_collision_; ///< Flag to know that we are receiving collision meassures.
+ 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.
// [publisher attributes]
@@ -101,12 +105,51 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
/**
* \brief Function to tranform linear local acceleration to fixed frame.
*
- * This checks if imu frame id is different from fixed frame and if not already
+ * It checks if imu frame id is different from fixed frame and if not already
* saved, it saves the transfrom.
*
* \param _global_acc The current global linear acceleration.
+ *
+ * \return True in success.
*/
bool get_global_acc(Eigen::Vector3d& _global_acc);
+
+ /**
+ * \brief Function to check if is a collision transition.
+ *
+ * It checks if the current acceleration meassured is behind or above the threshold
+ * (depending on _start param) updates the counter and checks if the limit has been reached.
+ *
+ * \param _acc_norm_2d The current global linear acceleration norm.
+ *
+ * \param _start If it must check the star transition.
+ *
+ * \return True if the conditions has been met.
+ */
+ bool check_collision_transition(double _acc_norm_2d, bool _start);
+
+ /**
+ * \brief Function to check the end of a collision.
+ *
+ * It calls check_collision_transition with _start to false.
+ *
+ * \param _acc_norm_2d The current global linear acceleration norm.
+ *
+ * \return True if the condition has been met.
+ */
+ bool check_end_of_collision(double _acc_norm_2d);
+
+ /**
+ * \brief Function to check the end of a collision.
+ *
+ * It calls check_collision_transition with _start to true.
+ *
+ * \param _acc_norm_2d The current global linear acceleration norm.
+ *
+ * \return True if the condition has been met.
+ */
+ bool check_start_of_collision(double _acc_norm_2d);
+
/**
* \brief main node thread
*
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index d578d06866751a2b9eb7246c78418fbb69003bc3..e54c4e6934d846362d0b549b6d485b9e86ba079f 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -21,6 +21,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
this->new_imu_input_ = false;
this->imu_frame_ = "";
this->tf_loaded_ = false;
+ this->in_collision_ = false;
+ this->low_acc_count_ = 0;
// [init publishers]
@@ -57,13 +59,30 @@ void CollisionManagerAlgNode::mainNodeThread(void)
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_angle_2d = std::atan2(global_acc(1), global_acc(0));
- while (acc_angle_2d >= M_PI)
- acc_angle_2d -= 2*M_PI;
- while (acc_angle_2d < -M_PI)
- acc_angle_2d += 2*M_PI;
- if (acc_norm_2d > this->config_.collision_acc_th)
- ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << acc_norm_2d << "; angle = " << acc_angle_2d);
+ 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));
+ if (check_end_of_collision(acc_norm_2d))
+ {
+ double collision_angle = std::atan2(this->max_acc_(1), this->max_acc_(0));
+ while (collision_angle >= M_PI)
+ collision_angle -= 2*M_PI;
+ while (collision_angle < -M_PI)
+ collision_angle += 2*M_PI;
+ ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected.");
+ ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
+ this->in_collision_ = false;
+ this->low_acc_count_ = 0;
+ }
+ else if (acc_norm_2d > max_acc_norm_2d)
+ this->max_acc_ = global_acc;
+ }
+ else if (check_start_of_collision(acc_norm_2d))
+ {
+ ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
+ this->in_collision_ = true;
+ this->max_acc_ = global_acc;
+ }
}
this->new_imu_input_ = false;
}
@@ -146,6 +165,32 @@ bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc)
return true;
}
+bool CollisionManagerAlgNode::check_collision_transition(double _acc_norm_2d, bool _start)
+{
+ if (this->config_.collision_transition_counter_en)
+ {
+ if ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
+ this->low_acc_count_++;
+ else
+ {
+ this->low_acc_count_ = 0;
+ return false;
+ }
+ }
+ return ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
+ && (_start || (!_start && (!this->config_.collision_transition_counter_en || (this->low_acc_count_ >= this->config_.collision_counter_limit))));
+}
+
+bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d)
+{
+ return check_collision_transition(_acc_norm_2d, false);
+}
+
+bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d)
+{
+ return check_collision_transition(_acc_norm_2d, true);
+}
+
/* [subscriber callbacks] */
void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
@@ -157,6 +202,8 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg
this->new_imu_input_ = true;
this->imu_frame_ = msg->header.frame_id;
this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
+ if (!this->in_collision_)
+ this->collision_start_stamp_ = msg->header.stamp;
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();