Added calculate_ang_vel param. Fixed bug on ICP odometry calculation

cfg/CollisionManager.cfg

@@ -46,6 +46,7 @@ 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("calculate_ang_vel",                    bool_t,     0,           "Flag to calculate robot angular vel from odometry",     False)
 
 collision_detection.add("collision_timeout",                        double_t,   0,           "Timeout to detect an end of collision",                            2.0, 1.0, 30.0)
 collision_detection.add("collision_acc_transition_counter_en",      bool_t,     0,           "Enable the collision counter for the acc detection",               False)

config/params.yaml

@@ -4,6 +4,7 @@ fixed_frame: map
 base_link_frame: base_link
 tf_timeout: 0.2
 err_msg_rate: 0.5
+calculate_ang_vel: True
 
 collision_timeout: 4.0
 collision_acc_transition_counter_en: True
@@ -13,13 +14,13 @@ collision_ang_vel_transition_counter_en: True
 collision_ang_vel_th: 0.4
 #collision_ang_vel_th: 0.2
 collision_ang_vel_counter_limit: 20
-odom_trans_th: 0.1
+odom_trans_th: 0.2
 odom_angular_th: 0.15
 
 front_laser_en: True
 rear_laser_en: True
 buffer_size: 10
-scan_diff_t_from_collision: 0.15
+scan_diff_t_from_collision: 0.1
 front_laser_en: True
 rear_laser_en: True
 icp_min_points: 280

include/collision_manager_alg_node.h

@@ -92,6 +92,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     ros::Time collision_start_stamp_; ///< Time stamp of a collision initial detection.
     // 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.
+    double max_ang_vel_diff_; ///< Maximum angular velocity difference between the odometry and the IMU.
     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.
@@ -210,13 +211,13 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      * It checks if imu frame id is different from base_link frame and if not already 
      * saved, it saves the transfrom.
      * 
-     * \param _global_acc The current global linear acceleration.
+     * \param _blink_acc The current global linear acceleration.
      * 
-     * \param _global_ang_vel The current global angular velocity.
+     * \param _blink_ang_vel The current global angular velocity.
      * 
      * \return True in success.
      */
-    bool get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
+    bool get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
 
     /**
      * \brief Function to check if is an acceleration collision transition.

msg/collision.msg

@@ -4,7 +4,7 @@ float32 acc_peak                        # Acceleration peak at collision
 float32 acc_angle                       # Estimation of the collision angle from accelerometer data.
 
 bool ang_vel_trigger                    # If angular velocity has trigger the collision detection.
-float32 ang_vel_diff                    # Angular velocity difference between IMU data and odometry.
+float32 ang_vel_diff_peak               # Angular velocity difference peak between IMU data and odometry.
 
 bool front_icp_check                    # If front ICP checks that is a collision.
 int32 front_icp_points                  # Front ICP laser scan matching points.

src/collision_manager_alg_node.cpp

 #include "collision_manager_alg_node.h"
+#include <math.h>  
 
 CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>()
@@ -124,18 +125,17 @@ void CollisionManagerAlgNode::mainNodeThread(void)
             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);
+          // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision end detected.");
+          // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << max_acc_norm_2d << "; angle = " << collision_angle);
 
           //update collision msg data
           this->collision_msg_.acc_peak = max_acc_norm_2d;
           this->collision_msg_.acc_angle = collision_angle;
+          this->collision_msg_.ang_vel_diff_peak = this->max_ang_vel_diff_;
 
           //update rot_angle_estimated
+          // TODO: rotate angle with tf between blink and laser
           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);
@@ -191,33 +191,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                   this->tf2_broadcaster.sendTransform(this->transform_msg);
                 }
 
-                if (front_icp_srv_.response.nvalid > this->config_.icp_min_points && front_icp_srv_.response.error/front_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points)
+                if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
                 {
-                  if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
-                  {
-                    icp_odom_calculated = true;
-                    ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
-                    ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
-                    ROS_INFO_STREAM("New laser pose " << new_laser_pose);
-                    ROS_INFO_STREAM("ICP odom " << icp_odometry);
-                    ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
-                    icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
-
-                    // Update collision msg
-                    this->collision_msg_.front_odom_diff.x = diff_wheel_icp_odom(0);
-                    this->collision_msg_.front_odom_diff.y = diff_wheel_icp_odom(1);
-                    this->collision_msg_.front_odom_diff.theta = diff_wheel_icp_odom(2);
-                    for (unsigned int i=0; i<9; i++)
-                      this->collision_msg_.front_covariance[i] = icp_odom_cov(i);
-                  }
-                  else
+                  icp_odom_calculated = true;
+                  // ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
+                  // ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
+                  // ROS_INFO_STREAM("New laser pose " << new_laser_pose);
+                  // ROS_INFO_STREAM("ICP odom " << icp_odometry);
+                  // ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
+                  // ROS_INFO_STREAM("odom diff " << this->odom_after_collision_ - this->odom_before_collision_);
+                  icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
+
+                  // Update collision msg
+                  this->collision_msg_.front_odom_diff.x = diff_wheel_icp_odom(0);
+                  this->collision_msg_.front_odom_diff.y = diff_wheel_icp_odom(1);
+                  this->collision_msg_.front_odom_diff.theta = diff_wheel_icp_odom(2);
+                  for (unsigned int i=0; i<9; i++)
+                    this->collision_msg_.front_covariance[i] = icp_odom_cov(i);
+                  if (!(front_icp_srv_.response.nvalid > this->config_.icp_min_points && front_icp_srv_.response.error/front_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points))
                   {
-                    this->collision_msg_.front_odom_diff.x = 0.0;
-                    this->collision_msg_.front_odom_diff.y = 0.0;
-                    this->collision_msg_.front_odom_diff.theta = 0.0;
-                    for (unsigned int i=0; i<9; i++)
-                      this->collision_msg_.front_covariance[i] = 0.0;
-                    icp_odom_calculated = false;
+                    ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Not enough points or too much error on front ICP. Not valid to check if is a collision");
                     icp_is_collision = false;
                   }
                 }
@@ -303,33 +296,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                   this->tf2_broadcaster.sendTransform(this->transform_msg);
                 }
 
-                if (rear_icp_srv_.response.nvalid > this->config_.icp_min_points && rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points)
+                if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
                 {
-                  if (get_icp_odometry(new_laser_pose, covariances, icp_odometry, icp_odom_cov, icp_odometry_diff, true))
-                  {
-                    icp_odom_calculated = true;
-                    ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
-                    ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
-                    ROS_INFO_STREAM("New laser pose " << new_laser_pose);
-                    ROS_INFO_STREAM("ICP odom " << icp_odometry);
-                    ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
-                    icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
-
-                    // Update collision msg
-                    this->collision_msg_.rear_odom_diff.x = diff_wheel_icp_odom(0);
-                    this->collision_msg_.rear_odom_diff.y = diff_wheel_icp_odom(1);
-                    this->collision_msg_.rear_odom_diff.theta = diff_wheel_icp_odom(2);
-                    for (unsigned int i=0; i<9; i++)
-                      this->collision_msg_.rear_covariance[i] = icp_odom_cov(i);
-                  }
-                  else
+                  icp_odom_calculated = true;
+                  // ROS_INFO_STREAM("Odom before collision " << this->odom_before_collision_);
+                  // ROS_INFO_STREAM("Odom after collision " << this->odom_after_collision_);
+                  // ROS_INFO_STREAM("New laser pose " << new_laser_pose);
+                  // ROS_INFO_STREAM("ICP odom " << icp_odometry);
+                  // ROS_INFO_STREAM("ICP odom diff " << icp_odometry_diff);
+                  // ROS_INFO_STREAM("odom diff " << this->odom_after_collision_ - this->odom_before_collision_);
+                  icp_is_collision = check_odometry_diff(icp_odometry_diff, diff_wheel_icp_odom);
+
+                  // Update collision msg
+                  this->collision_msg_.rear_odom_diff.x = diff_wheel_icp_odom(0);
+                  this->collision_msg_.rear_odom_diff.y = diff_wheel_icp_odom(1);
+                  this->collision_msg_.rear_odom_diff.theta = diff_wheel_icp_odom(2);
+                  for (unsigned int i=0; i<9; i++)
+                    this->collision_msg_.rear_covariance[i] = icp_odom_cov(i);
+                  if (!(rear_icp_srv_.response.nvalid > this->config_.icp_min_points && rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid < this->config_.icp_max_err_by_points))
                   {
-                    this->collision_msg_.rear_odom_diff.x = 0.0;
-                    this->collision_msg_.rear_odom_diff.y = 0.0;
-                    this->collision_msg_.rear_odom_diff.theta = 0.0;
-                    for (unsigned int i=0; i<9; i++)
-                      this->collision_msg_.rear_covariance[i] = 0.0;
-                    icp_odom_calculated = false;
+                    ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Not enough points or too much error on rear ICP. Not valid to check if is a collision");
                     icp_is_collision = false;
                   }
                 }
@@ -370,9 +356,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
             }
             // fuse poses?? call service to modify SLAM graph
           }
-          ROS_INFO_STREAM("");
-          ROS_INFO_STREAM("");
-          ROS_INFO_STREAM("");
           //restart variables
           this->in_collision_ = false;
           this->low_acc_count_ = 0;
@@ -388,6 +371,8 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           //Update max_acc
           if (acc_norm_2d > max_acc_norm_2d)
             this->max_acc_ = blink_acc;
+          if (std::fabs(blink_ang_vel(2) - this->angular_vel_) > this->max_ang_vel_diff_)
+            this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2));
 
           //Update angle rotated
           this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
@@ -405,7 +390,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
       }
       else if (check_start_of_collision(acc_norm_2d, std::fabs(blink_ang_vel(2))))
       {
-        ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
+        // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision start detected.");
         this->collision_start_stamp_ = this->imu_stamp_;
 
         // Get reference scan
@@ -427,13 +412,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
         //init variables
         this->in_collision_ = true;
         this->max_acc_ = blink_acc;
+        this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2) - this->angular_vel_);
         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;
 
         //update collision msg data
         this->collision_msg_.header.stamp = this->imu_stamp_;
-        this->collision_msg_.ang_vel_diff = std::fabs(blink_ang_vel(2));
       }
     }
     this->new_imu_input_ = false;
@@ -583,26 +568,26 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   // this->alg_.unlock();
 }
 
-bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel)
+bool CollisionManagerAlgNode::get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel)
 {
-  if (this->config_.base_link_frame != this->imu_frame_ || !this->tf_base_link_imu_loaded_)
+  if (!this->tf_base_link_imu_loaded_)
   {
     geometry_msgs::TransformStamped tf_base_link_imu_msg;
-    if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
+    if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->imu_frame_, this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
     {
-      ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_imu_data -> Can't transform from " << this->config_.base_link_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
+      ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_imu_data -> Can't transform from " << this->config_.base_link_frame << " to " << this->imu_frame_ << " at " << this->imu_stamp_);
       this->tf_base_link_imu_loaded_ = false;
       return false;
     }
-    tf_base_link_imu_msg = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->imu_frame_, ros::Time::now());
+    tf_base_link_imu_msg = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->imu_frame_, this->imu_stamp_);
     Eigen::Quaternion<double> rot(
     tf_base_link_imu_msg.transform.rotation.w, tf_base_link_imu_msg.transform.rotation.x, tf_base_link_imu_msg.transform.rotation.y, tf_base_link_imu_msg.transform.rotation.z);
     
     this->tf_base_link_imu_ = Eigen::Transform<double,3,Eigen::Affine>(rot);
     this->tf_base_link_imu_loaded_ = true;
   }
-  _global_acc = this->tf_base_link_imu_*this->imu_local_acc_;
-  _global_ang_vel = this->tf_base_link_imu_*this->imu_local_ang_vel_;
+  _blink_acc = this->tf_base_link_imu_*this->imu_local_acc_;
+  _blink_ang_vel = this->tf_base_link_imu_*this->imu_local_ang_vel_;
   return true;
 }
 
@@ -656,8 +641,12 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
   // update collision msg
   if (acc_true)
     this->collision_msg_.acc_trigger = true;
+  else
+    this->collision_msg_.acc_trigger = false;
   if (ang_vel_true)
     this->collision_msg_.ang_vel_trigger = true;
+  else
+    this->collision_msg_.ang_vel_trigger = false;
   // 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)
@@ -745,18 +734,19 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
 {
   geometry_msgs::TransformStamped tf_aux;
   tf2::Transform tf_map_laser, tf_map_new_laser, tf_laser_new_laser, tf_map_new_blink, tf_laser_map;
+  ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
 
   if (!load_tf_blink_laser(_front))
     return false;
 
-  if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+  if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), t, ros::Duration(this->config_.tf_timeout)))
   {
-    ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_new_pose_from_icp -> Can't transform from " << this->config_.fixed_frame << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << this->imu_stamp_);
+    ROS_WARN_STREAM("CollisionManagerAlgNode::get_base_link_new_pose_from_icp -> Can't transform from " << this->config_.fixed_frame << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << t);
     return false;
   }
   else
   {
-    tf_aux = this->tf2_buffer.lookupTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
+    tf_aux = this->tf2_buffer.lookupTransform(this->config_.fixed_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), t);
     tf2::fromMsg(tf_aux.transform, tf_map_laser);
   }
 
@@ -815,19 +805,35 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
 bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _odom, Eigen::Matrix3d& _odom_cov, Eigen::Vector3d& _odom_diff, const bool _front)
 {
   geometry_msgs::TransformStamped tf_aux;
-  tf2::Transform tf_odom_laser, tf_odom_new_laser, tf_laser_new_laser, tf_odom_new_blink, tf_laser_odom, tf_odom_new_laser_diff, rot_odom_laser;
+  tf2::Transform tf_odom_laser, tf_odom_new_laser, tf_laser_new_laser, tf_odom_new_blink, tf_laser_odom, tf_odom_blink;
+  Eigen::Vector3d old_odom;
+  ros::Time t = this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision);
 
   if (!load_tf_blink_laser(_front))
     return false;
 
-  if (!this->tf2_buffer.canTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_, ros::Duration(this->config_.tf_timeout)))
+  if (!this->tf2_buffer.canTransform(this->odom_frame_, this->config_.base_link_frame, t, ros::Duration(this->config_.tf_timeout)))
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::get_icp_odometry -> Can't transform from " << this->odom_frame_ << " to " << this->config_.base_link_frame << " at " << t);
+    return false;
+  }
+  else
+  {
+    tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, this->config_.base_link_frame, t);
+    tf2::fromMsg(tf_aux.transform, tf_odom_blink);
+  }
+  old_odom << tf_odom_blink.getOrigin().x(), tf_odom_blink.getOrigin().y(), tf2::getYaw(tf_odom_blink.getRotation());
+
+  // ROS_INFO_STREAM("ICP odom before " << old_odom);
+
+  if (!this->tf2_buffer.canTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), t, ros::Duration(this->config_.tf_timeout)))
   {
-    ROS_WARN_STREAM("CollisionManagerAlgNode::get_icp_odometry -> Can't transform from " << this->odom_frame_ << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << this->imu_stamp_);
+    ROS_WARN_STREAM("CollisionManagerAlgNode::get_icp_odometry -> Can't transform from " << this->odom_frame_ << " to " << (_front? this->front_laser_frame_: this->rear_laser_frame_) << " at " << t);
     return false;
   }
   else
   {
-    tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
+    tf_aux = this->tf2_buffer.lookupTransform(this->odom_frame_, (_front? this->front_laser_frame_: this->rear_laser_frame_), t);
     tf2::fromMsg(tf_aux.transform, tf_odom_laser);
   }
 
@@ -848,9 +854,11 @@ bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser
   for (unsigned int i=0; i<9; i++)
     _odom_cov(i) = std::fabs(_odom_cov(i))*(_covariances(i) < 0? -1: 1);
 
-  rot_odom_laser = tf2::Transform(tf_odom_laser.getRotation(), tf2::Vector3(0.0, 0.0, 0.0));
-  tf_odom_new_laser_diff = rot_odom_laser*tf_laser_new_laser;
-  _odom_diff <<  tf_odom_new_laser_diff.getOrigin().x(), tf_odom_new_laser_diff.getOrigin().y(), _new_laser_pose(2);
+  _odom_diff = _odom - old_odom;
+  while (_odom_diff(2) >= M_PI)
+    _odom_diff(2) -= 2*M_PI;
+  while (_odom_diff(2) < -M_PI)
+    _odom_diff(2) += 2*M_PI;
 
   return true;
 }
@@ -886,8 +894,12 @@ bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_od
   Eigen::Vector3d limits;
   limits << this->config_.odom_trans_th, this->config_.odom_trans_th, this->config_.odom_angular_th;
   _icp_odometry_diff = _icp_odometry - (this->odom_after_collision_ - this->odom_before_collision_);
+  while (_icp_odometry_diff(2) >= M_PI)
+    _icp_odometry_diff(2) -= 2*M_PI;
+  while (_icp_odometry_diff(2) < -M_PI)
+    _icp_odometry_diff(2) += 2*M_PI;
 
-  Eigen::Matrix<bool, 3, 1> odom_check = _icp_odometry.array().abs() >= limits.array();
+  Eigen::Matrix<bool, 3, 1> odom_check = _icp_odometry_diff.array().abs() >= limits.array();
   return (odom_check(0) || odom_check(1) || odom_check(2));
 }
 
@@ -906,10 +918,29 @@ void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr&
   odom.odom(0) = msg->pose.pose.position.x;
   odom.odom(1) = msg->pose.pose.position.y;
   odom.odom(2) = tf2::getYaw(tf.getRotation());
+  if (this->config_.calculate_ang_vel)
+  {
+    double diff_ang = odom.odom(2) - this->odom_buffer_.front().odom(2);
+    while (diff_ang >= M_PI)
+      diff_ang -= 2*M_PI;
+    while (diff_ang < -M_PI)
+      diff_ang += 2*M_PI;
+    this->angular_vel_ = (diff_ang)/(odom.stamp - this->odom_buffer_.front().stamp).toSec();
+  }
+  else
+    this->angular_vel_ = msg->twist.twist.angular.z;
+  // if (std::fabs(this->angular_vel_) > 0.5)
+  // {
+  //   ROS_INFO_STREAM("last odom: stamp " << this->odom_buffer_.front().stamp);
+  //   ROS_INFO_STREAM("last odom: odom " << this->odom_buffer_.front().odom);
+  //   ROS_INFO_STREAM("current odom: stamp " << odom.stamp);
+  //   ROS_INFO_STREAM("current odom: odom " << odom.odom);
+  //   ROS_INFO_STREAM("ang vel = " << this->angular_vel_);
+  // }
+
   if (this->odom_buffer_.size() >= this->config_.buffer_size)
     this->odom_buffer_.pop_back();
   this->odom_buffer_.push_front(odom);
-  this->angular_vel_ = msg->twist.twist.angular.z;
   this->odom_frame_ = msg->header.frame_id;
 
   //std::cout << msg->data << std::endl;