Added angular velocity collision detector

README.md

@@ -26,15 +26,21 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
   - ~**icp** (iri_laser_scan_icp/icp.srv): ICP server.
 
 ### Parameters
+
+##### Generals
 - ~**rate** (Double; default: 10.0; min: 0.1; max: 1000): The main node thread loop rate in Hz. 
 - ~**debug** (Boolean; default: False): Boolean to publish debug information.
 - ~**fixed_frame** (String; default: base_link): Fixed frame id.
 - ~**tf_timeout** (Double; default: 0.2; min: 0.1; max: 2.0): Timeout to find a transform.
 - ~**err_msg_rate** (Double; default: 0.5; min: 0.1; max: 1.0): Rate to publish error messages.
-- ~**collision_transition_counter_en** (Boolean; default: False): Enable the collision counter. 
+
+##### Collision detection
 - ~**collision_timeout** (Double; default: 4.0; min: 1.0; max: 10.0): Timeout to detect an end of collision.
+- ~**collision_acc_transition_counter_en** (Boolean; default: False): Enable the collision counter. 
 - ~**collision_acc_th** (Double; default: 9.8; min: 0.1; max: 160.0): Threshold to detect a collision.
-- ~**collision_counter_limit** (Integer; default: 6; min: 1; max: 30): Number of low acc imu meassures to detect an end of collision.
+- ~**collision_acc_counter_limit** (Integer; default: 6; min: 1; max: 30): Number of low acc imu meassures to detect an end of collision.
+
+##### ICP data
 - ~**front_laser_en** (Boolean; default: True): Enable front laser for ICP.
 - ~**rear_laser_en** (Boolean; default: True): Enable rear laser for ICP.
 - ~**scan_buffer_size** (Integer; default: 3; min: 1; max: 10): Number of laser_scan buffered for the ICP.

cfg/CollisionManager.cfg

@@ -36,6 +36,8 @@ PACKAGE='iri_collision_manager'
 from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
+collision_detection = gen.add_group("Collision detection parameters")
+icp = gen.add_group("ICP data parameters")
 
 #       Name     Type       Reconf.level Description             Default Min   Max
 gen.add("rate",                                 double_t,   0,           "Main loop rate (Hz)",                                   10.0,   0.1, 1000.0)
@@ -44,16 +46,21 @@ gen.add("err_msg_rate",                         double_t,   0,           "Rate t
 gen.add("fixed_frame",                          str_t,      0,           "Fixed frame",                                           "map")
 gen.add("base_link_frame",                      str_t,      0,           "Base_link frame",                                       "base_link")
 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_timeout",                    double_t,   0,           "Timeout to detect an end of collision",                 2.0, 1.0, 10.0)
-gen.add("collision_acc_th",                     double_t,   0,           "Threshold to detect a collision",                       9.8,    0.1, 160.0)
-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("scan_buffer_size",                     int_t,      0,           "Number of laser_scan buffered for the ICP",             3, 1, 10)
-gen.add("scan_diff_t_from_collision",           double_t,   0,           "Time to subtract from collision time stamp to ensure a clean scan",   0.0,    0.0, 0.5)
-gen.add("front_laser_en",                       bool_t,     0,           "Enable front laser for ICP",                            True)
-gen.add("rear_laser_en",                        bool_t,     0,           "Enable rear laser for ICP",                             True)
-gen.add("icp_min_points",                       int_t,      0,           "Minimum ICP points thershold",                          300,    0, 2000)
-gen.add("icp_max_err_by_points",                double_t,   0,           "Maximum ICP average error per point",                   0.1,    0.0, 10.0)
+
+collision_detection.add("collision_timeout",                        double_t,   0,           "Timeout to detect an end of collision",                            2.0, 1.0, 10.0)
+collision_detection.add("collision_acc_transition_counter_en",      bool_t,     0,           "Enable the collision counter for the acc detection",               False)
+collision_detection.add("collision_acc_th",                         double_t,   0,           "Acceleration threshold to detect a collision",                     9.8,    0.1, 160.0)
+collision_detection.add("collision_acc_counter_limit",              int_t,      0,           "Number of low acc imu meassures to detect an end of collision",    6, 1, 30)
+collision_detection.add("collision_ang_vel_transition_counter_en",  bool_t,     0,           "Enable the collision counter",                                     False)
+collision_detection.add("collision_ang_vel_th",                     double_t,   0,           "Angular velocity threshold to detect a collision",                 0.3,    0.1, 1)
+collision_detection.add("collision_ang_vel_counter_limit",          int_t,      0,           "Number of low acc imu meassures to detect an end of collision",    3, 1, 30)
+
+icp.add("scan_buffer_size",                     int_t,      0,           "Number of laser_scan buffered for the ICP",             3, 1, 10)
+icp.add("scan_diff_t_from_collision",           double_t,   0,           "Time to subtract from collision time stamp to ensure a clean scan",   0.0,    0.0, 0.5)
+icp.add("front_laser_en",                       bool_t,     0,           "Enable front laser for ICP",                            True)
+icp.add("rear_laser_en",                        bool_t,     0,           "Enable rear laser for ICP",                             True)
+icp.add("icp_min_points",                       int_t,      0,           "Minimum ICP points thershold",                          300,    0, 2000)
+icp.add("icp_max_err_by_points",                double_t,   0,           "Maximum ICP average error per point",                   0.1,    0.0, 10.0)
 
 
 exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file

config/params.yaml

@@ -4,10 +4,17 @@ fixed_frame: map
 base_link_frame: base_link
 tf_timeout: 0.2
 err_msg_rate: 0.5
-collision_transition_counter_en: True
+
 collision_timeout: 4.0
-collision_acc_th: 2.5
-collision_counter_limit: 12
+collision_acc_transition_counter_en: True
+collision_acc_th: 3.0
+#collision_acc_th: 2.0
+collision_acc_counter_limit: 12
+collision_ang_vel_transition_counter_en: True
+collision_ang_vel_th: 0.4
+#collision_ang_vel_th: 0.2
+collision_ang_vel_counter_limit: 4
+
 front_laser_en: True
 rear_laser_en: True
 scan_buffer_size: 6

include/collision_manager_alg_node.h

@@ -79,6 +79,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     // ros::Time collision_end_stamp_; ///< Time stamp at the end of collision.
     Eigen::Vector3d max_acc_; ///< The linear accelerations meassured from base_link frame reference at maximum acceleration module.
     int low_acc_count_; ///< Low acceleration meassures counter to detect the end of a collision.
+    int low_ang_vel_count_; ///< Low angular velocities meassures counter to detect the end of a collision.
     std::list<RangesWithStamp> front_ranges_; ///< Front laser ranges buffer.
     std::list<RangesWithStamp> rear_ranges_; ///< Rear laser ranges buffer.
     bool is_front_icp_input_data_ok_; ///<Boolean to know of all the necessary data has been saved on the service request.
@@ -182,7 +183,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     bool get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
 
     /**
-     * \brief Function to check if is a collision transition.
+     * \brief Function to check if is an acceleration 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. 
@@ -193,18 +194,35 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      * 
      * \return True if the conditions has been met.
      */
-    bool check_collision_transition(double _acc_norm_2d, bool _start);
+    bool check_acc_collision_transition(double _acc_norm_2d, bool _start);
+
+    /**
+     * \brief Function to check if is an acceleration 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 _ang_vel The current angular velocity.
+     * 
+     * \param _start If it must check the star transition.
+     * 
+     * \return True if the conditions has been met.
+     */
+    bool check_ang_vel_collision_transition(double _ang_vel, bool _start);
 
     /**
      * \brief Function to check the end of a collision.
      * 
-     * It calls check_collision_transition with _start to false.
+     * It calls check_acc_collision_transition and check_ang_vel_collision_transition
+     * with _start to false.
      * 
      * \param _acc_norm_2d The current global linear acceleration norm.
      * 
+     * \param _ang_vel The current angular velocity.
+     * 
      * \return True if the condition has been met.
      */
-    bool check_end_of_collision(double _acc_norm_2d);
+    bool check_end_of_collision(double _acc_norm_2d, double _ang_vel);
 
     /**
      * \brief Function to check the end of a collision.
@@ -213,9 +231,11 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      * 
      * \param _acc_norm_2d The current global linear acceleration norm.
      * 
+     * \param _ang_vel The current angular velocity.
+     * 
      * \return True if the condition has been met.
      */
-    bool check_start_of_collision(double _acc_norm_2d);
+    bool check_start_of_collision(double _acc_norm_2d, double _ang_vel);
 
     /**
      * \brief Function to find the last scan before a collision.

src/collision_manager_alg_node.cpp

@@ -31,6 +31,7 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   this->tf_base_link_imu_loaded_ = false;
   this->in_collision_ = false;
   this->low_acc_count_ = 0;
+  this->low_ang_vel_count_ = 0;
   this->is_front_icp_input_data_ok_ = false;
   this->is_rear_icp_input_data_ok_ = false;
   this->tf_base_link_front_laser_loaded_ = false;
@@ -94,18 +95,18 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   this->imu_mutex_enter();
   this->rear_laser_scan_mutex_enter();
   this->front_laser_scan_mutex_enter();
-  Eigen::Vector3d global_acc, global_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
+  Eigen::Vector3d blink_acc, blink_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
   Eigen::Matrix3d rear_cov, front_cov, covariances;
 
   if (this->new_imu_input_)
   {
-    if (get_base_link_imu_data(global_acc, global_ang_vel))//Transform data to fixed frame coordinates
+    if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to fixed frame coordinates
     {
-      double acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
+      double acc_norm_2d = std::sqrt(std::pow(blink_acc(0),2) + std::pow(blink_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));
-        if (check_end_of_collision(acc_norm_2d))
+        if (check_end_of_collision(acc_norm_2d, std::fabs(blink_ang_vel(2))))
         {
           double collision_angle = std::atan2(this->max_acc_(1), this->max_acc_(0));
           while (collision_angle >= M_PI)
@@ -116,8 +117,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
 
           //update rot_angle_estimated
-          this->front_icp_srv_.request.rot_angle_estimation += global_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
+          this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
+
+          //rotate angle when fixed tf
+
           this->rear_icp_srv_.request.rot_angle_estimation = this->front_icp_srv_.request.rot_angle_estimation;
+
           ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
 
           // Call the service
@@ -206,6 +211,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           //restart variables
           this->in_collision_ = false;
           this->low_acc_count_ = 0;
+          this->low_ang_vel_count_ = 0;
           this->is_front_icp_input_data_ok_ = false;
           this->is_rear_icp_input_data_ok_ = false;
         }
@@ -213,10 +219,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
         {
           //Update max_acc
           if (acc_norm_2d > max_acc_norm_2d)
-            this->max_acc_ = global_acc;
+            this->max_acc_ = blink_acc;
 
           //Update angle rotated
-          this->front_icp_srv_.request.rot_angle_estimation += global_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
+          this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
           this->last_imu_t_ = this->imu_stamp_;
 
           //Check collision timeout
@@ -225,10 +231,11 @@ void CollisionManagerAlgNode::mainNodeThread(void)
             ROS_WARN_STREAM("CollisionManagerAlgNode::mainNodeThread -> No end of collision detected after " << this->config_.collision_timeout << " seconds.");
             this->in_collision_ = false;
             this->low_acc_count_ = 0;
+            this->low_ang_vel_count_ = 0;
           }
         }
       }
-      else if (check_start_of_collision(acc_norm_2d))
+      else if (check_start_of_collision(acc_norm_2d, std::fabs(blink_ang_vel(2))))
       {
         ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
         this->collision_start_stamp_ = this->imu_stamp_;
@@ -244,7 +251,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
         //init variables
         this->in_collision_ = true;
-        this->max_acc_ = global_acc;
+        this->max_acc_ = blink_acc;
         this->last_imu_t_ = this->collision_start_stamp_;
         this->front_icp_srv_.request.rot_angle_estimation = 0.0;
         this->rear_icp_srv_.request.rot_angle_estimation = 0.0;
@@ -383,30 +390,56 @@ bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _global_ac
   return true;
 }
 
-bool CollisionManagerAlgNode::check_collision_transition(double _acc_norm_2d, bool _start)
+bool CollisionManagerAlgNode::check_acc_collision_transition(double _acc_norm_2d, bool _start)
 {
-  if (this->config_.collision_transition_counter_en)
+  if (!this->config_.collision_acc_transition_counter_en)
+    return true;
+  if ((_start && (_acc_norm_2d > this->config_.collision_acc_th)) || (!_start && (_acc_norm_2d < this->config_.collision_acc_th)))
+    this->low_acc_count_++;
+  else
   {
-    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;
-    }
+    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))));
+         && (_start || (!_start && (!this->config_.collision_acc_transition_counter_en || (this->low_acc_count_ >= this->config_.collision_acc_counter_limit))));
+}
+
+bool CollisionManagerAlgNode::check_ang_vel_collision_transition(double _ang_vel, bool _start)
+{
+  if (!this->config_.collision_ang_vel_transition_counter_en)
+    return true;
+  if ((_start && (_ang_vel > this->config_.collision_ang_vel_th)) || (!_start && (_ang_vel < this->config_.collision_ang_vel_th)))
+    this->low_ang_vel_count_++;
+  else
+  {
+    this->low_ang_vel_count_ = 0;
+    return false;
+  }
+  return ((_start && (_ang_vel > this->config_.collision_ang_vel_th)) || (!_start && (_ang_vel < this->config_.collision_ang_vel_th)))  
+         && (_start || (!_start && (!this->config_.collision_ang_vel_transition_counter_en || (this->low_ang_vel_count_ >= this->config_.collision_ang_vel_counter_limit))));
 }
 
-bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d)
+bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d, double _ang_vel)
 {
-  return check_collision_transition(_acc_norm_2d, false);
+  bool acc_true = check_acc_collision_transition(_acc_norm_2d, false);
+  bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, false);
+  if (acc_true)
+    ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+  if (ang_vel_true)
+    ROS_INFO_STREAM("CollisionManagerAlgNode::check_end_of_collision -> (END) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+  return acc_true && ang_vel_true;
 }
 
-bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d)
+bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, double _ang_vel)
 {
-  return check_collision_transition(_acc_norm_2d, true);
+  bool acc_true = check_acc_collision_transition(_acc_norm_2d, true);
+  bool ang_vel_true = check_ang_vel_collision_transition(_ang_vel, true);
+  if (acc_true)
+    ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Acc true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+  if (ang_vel_true)
+    ROS_INFO_STREAM("CollisionManagerAlgNode::check_start_of_collision -> (START) Angular vel true. acc: " << _acc_norm_2d << "; vel: " << _ang_vel);
+  return acc_true || ang_vel_true;
 }
 
 bool CollisionManagerAlgNode::set_ref_ranges(ros::Time _stamp, bool _front)