Added collison.msg msg publication

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -21,11 +21,10 @@ find_package(Eigen3 REQUIRED)
 #           Add topic, service and action definition here
 # ******************************************************************** 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  collision.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -42,10 +41,11 @@ find_package(Eigen3 REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs  
+  geometry_msgs # Or other packages containing msgs
+)
 
 # ******************************************************************** 
 #                 Add the dynamic reconfigure file 
@@ -61,7 +61,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs
+ CATKIN_DEPENDS iri_base_algorithm message_runtime nav_msgs geometry_msgs iri_laser_scan_icp tf2_ros sensor_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} nav_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} iri_laser_scan_icp_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)

README.md

@@ -10,6 +10,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 
 # ROS Interface
 ### Topic publishers
+  - ~**collisions** (iri_collision_manager/collision.msg)
   - ~**debug_rear_after_icp_scan** (sensor_msgs/LaserScan.msg)
   - ~**debug_front_after_icp_scan** (sensor_msgs/LaserScan.msg)
   - /**tf** (tf/tfMessage)
@@ -20,6 +21,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
   - ~**debug_front_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize front ICP matching laser scan.
   - ~**debug_front_icp_ref_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize front ICP reference laser scan.
 ### Topic subscribers
+  - ~**odom** (nav_msgs/Odometry.msg)
   - ~**rear_laser_scan** (sensor_msgs/LaserScan.msg): Rear laser scan input data.
   - ~**front_laser_scan** (sensor_msgs/LaserScan.msg): Front laser scan input data.
   - /**tf** (tf/tfMessage): Robot tf.

include/collision_manager_alg_node.h

@@ -31,6 +31,8 @@
 #include <tf2/utils.h>
 
 // [publisher subscriber headers]
+#include <iri_collision_manager/collision.h>
+#include <nav_msgs/Odometry.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <sensor_msgs/LaserScan.h>
@@ -87,8 +89,12 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     bool is_rear_icp_input_data_ok_; ///<Boolean to know of all the necessary data has been saved on the service request.
     bool front_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
     bool rear_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
+    double angular_vel_; ///< Robot's angular velocity from odometry.
 
     // [publisher attributes]
+    ros::Publisher collisions_publisher_;
+    iri_collision_manager::collision collision_msg_;
+
     ros::Publisher debug_rear_after_icp_scan_publisher_;
     sensor_msgs::LaserScan debug_rear_after_icp_scan_msg_;
 
@@ -118,6 +124,12 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
 
 
     // [subscriber attributes]
+    ros::Subscriber odom_subscriber_;
+    void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+    pthread_mutex_t odom_mutex_;
+    void odom_mutex_enter(void);
+    void odom_mutex_exit(void);
+
     ros::Subscriber rear_laser_scan_subscriber_;
     void rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
     pthread_mutex_t rear_laser_scan_mutex_;

launch/collisions.launch

@@ -27,6 +27,7 @@
   <arg name="collision_config_file"   default="$(find iri_collision_manager)/config/params.yaml"/>
   <arg name="front_laser_topic"       default="/laser_front/scan"/>
   <arg name="rear_laser_topic"        default="/laser_rear/scan"/>
+  <arg name="odom_topic"              default="/odom"/>
 
   <include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
            if="$(arg imu_driver)">
@@ -65,6 +66,7 @@
     <arg name="front_laser_topic" value="$(arg front_laser_topic)"/>
     <arg name="rear_laser_topic" value="$(arg rear_laser_topic)"/>
     <arg name="icp_service_name" value="$(arg icp_service_name)"/>
+    <arg name="odom_topic" value="$(arg odom_topic)"/>
   </include>
 
 

launch/node.launch

@@ -10,6 +10,7 @@
   <arg name="front_laser_topic"     default="~/front_laser_scan"/>
   <arg name="rear_laser_topic"     default="~/rear_laser_scan"/>
   <arg name="icp_service_name"        default="~/icp"/>
+  <arg name="odom_topic"     default="~/odom"/>
 
   <node name="$(arg node_name)"
         pkg ="iri_collision_manager"
@@ -22,6 +23,7 @@
     <remap from="~/front_laser_scan" to="$(arg front_laser_topic)"/>
     <remap from="~/rear_laser_scan" to="$(arg rear_laser_topic)"/>
     <remap from="~/icp" to="$(arg icp_service_name)"/>
+    <remap from="~/odom" to="$(arg odom_topic)"/>
   </node>
 
 </launch>

launch/test.launch

@@ -19,6 +19,7 @@
     <arg name="front_laser_topic" value="/front_laser_scan"/>
     <arg name="rear_laser_topic" value="/rear_laser_scan"/>
     <arg name="icp_service_name" value="~/icp"/>
+    <arg name="odom_topic" value="/odom"/>
   </include>
 
   <node name="rqt_reconfigure_iri_collision_manager"

msg/collision.msg

+Header header                           # Msg header.
+float32 acc_peak                        # Acceleration peak at collision
+float32 acc_angle                       # Estimation of the collision angle from accelerometer data.
+
+float32 ang_vel_diff                    # Angular velocity difference between IMU data and odometry.
+
+int32 front_icp_points                  # Front ICP laser scan matching points.
+float32 front_icp_err                   # Front ICP error per point.
+geometry_msgs/Pose2D front_laser_disp   # Front laser displacement.
+float32[9] front_covariance             # Front laser displacement covariance.
+
+int32 rear_icp_points                   # Rear ICP laser scan matching points.
+float32 rear_icp_err                    # Rear ICP error per point.
+geometry_msgs/Pose2D rear_laser_disp    # Rear laser displacement.
+float32[9] rear_covariance              # Rear laser displacement covariance.

package.xml

@@ -51,7 +51,10 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_algorithm</build_depend>
   <build_export_depend>iri_base_algorithm</build_export_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
   <exec_depend>iri_base_algorithm</exec_depend>
+  <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>iri_laser_scan_icp</depend>
   <depend>tf2_ros</depend>

src/collision_manager_alg_node.cpp

@@ -38,6 +38,7 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   this->tf_base_link_rear_laser_loaded_ = false;
   this->front_icp_succedded_ = false;
   this->rear_icp_succedded_ = false;
+  this->angular_vel_ = 0.0;
 
   this->debug_rear_icp_pose_msg_.header.frame_id = this->config_.fixed_frame;
   this->debug_rear_icp_pose_msg_.pose.pose.position.z = 0.0;
@@ -49,7 +50,10 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   for (unsigned int i = 0; i < 36; i++)
     this->debug_front_icp_pose_msg_.pose.covariance[i] = 0.0;
 
+  this->collision_msg_.header.frame_id = "iri_collision_manager";
+
   // [init publishers]
+  this->collisions_publisher_ = this->private_node_handle_.advertise<iri_collision_manager::collision>("collisions", 1);
   this->debug_rear_after_icp_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_rear_after_icp_scan", 1);
   this->debug_front_after_icp_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_front_after_icp_scan", 1);
   this->debug_rear_icp_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("debug_rear_icp_pose", 1);
@@ -60,6 +64,9 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   this->debug_front_icp_ref_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_front_icp_ref_scan", 1);
   
   // [init subscribers]
+  this->odom_subscriber_ = this->private_node_handle_.subscribe("odom", 1, &CollisionManagerAlgNode::odom_callback, this);
+  pthread_mutex_init(&this->odom_mutex_,NULL);
+
   this->rear_laser_scan_subscriber_ = this->private_node_handle_.subscribe("rear_laser_scan", 1, &CollisionManagerAlgNode::rear_laser_scan_callback, this);
   pthread_mutex_init(&this->rear_laser_scan_mutex_,NULL);
 
@@ -84,6 +91,7 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
 CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
 {
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->odom_mutex_);
   pthread_mutex_destroy(&this->rear_laser_scan_mutex_);
   pthread_mutex_destroy(&this->front_laser_scan_mutex_);
   pthread_mutex_destroy(&this->imu_mutex_);
@@ -97,6 +105,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   this->imu_mutex_enter();
   this->rear_laser_scan_mutex_enter();
   this->front_laser_scan_mutex_enter();
+  this->odom_mutex_enter();
   Eigen::Vector3d blink_acc, blink_ang_vel, rear_blink_pose, front_blink_pose, new_laser_pose;
   Eigen::Matrix3d rear_cov, front_cov, covariances;
 
@@ -118,6 +127,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           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;
+
           //update rot_angle_estimated
           this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
 
@@ -125,7 +138,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
           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);
+          // ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: angle estimated = " << this->front_icp_srv_.request.rot_angle_estimation);
 
           // Call the service
           if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
@@ -136,13 +149,20 @@ void CollisionManagerAlgNode::mainNodeThread(void)
             {
               if (icp_client_.call(front_icp_srv_))
               {
-                ROS_INFO("CollisionManagerAlgNode::mainNodeThread -> Front icp_client_ received a response from service server");
-                ROS_INFO_STREAM("   valid " << (int)front_icp_srv_.response.valid);
-                ROS_INFO_STREAM("   new_laser_pose " << front_icp_srv_.response.new_laser_pose);
-                ROS_INFO_STREAM("   covariance " << front_icp_srv_.response.covariance[0] << ", " << front_icp_srv_.response.covariance[1] << ", " << front_icp_srv_.response.covariance[2] << ", " << front_icp_srv_.response.covariance[3] << ", " << front_icp_srv_.response.covariance[4] << ", " << front_icp_srv_.response.covariance[5] << ", " << front_icp_srv_.response.covariance[6] << ", " << front_icp_srv_.response.covariance[7] << ", " << front_icp_srv_.response.covariance[8]);
-                ROS_INFO_STREAM("   nvalid " << front_icp_srv_.response.nvalid << "; min " << this->config_.icp_min_points);
-                ROS_INFO_STREAM("   error " << front_icp_srv_.response.error);
-                ROS_INFO_STREAM("   error/points " << front_icp_srv_.response.error/front_icp_srv_.response.nvalid << "; max " << this->config_.icp_max_err_by_points);
+                // ROS_INFO("CollisionManagerAlgNode::mainNodeThread -> Front icp_client_ received a response from service server");
+                // ROS_INFO_STREAM("   valid " << (int)front_icp_srv_.response.valid);
+                // ROS_INFO_STREAM("   new_laser_pose " << front_icp_srv_.response.new_laser_pose);
+                // ROS_INFO_STREAM("   covariance " << front_icp_srv_.response.covariance[0] << ", " << front_icp_srv_.response.covariance[1] << ", " << front_icp_srv_.response.covariance[2] << ", " << front_icp_srv_.response.covariance[3] << ", " << front_icp_srv_.response.covariance[4] << ", " << front_icp_srv_.response.covariance[5] << ", " << front_icp_srv_.response.covariance[6] << ", " << front_icp_srv_.response.covariance[7] << ", " << front_icp_srv_.response.covariance[8]);
+                // ROS_INFO_STREAM("   nvalid " << front_icp_srv_.response.nvalid << "; min " << this->config_.icp_min_points);
+                // ROS_INFO_STREAM("   error " << front_icp_srv_.response.error);
+                // ROS_INFO_STREAM("   error/points " << front_icp_srv_.response.error/front_icp_srv_.response.nvalid << "; max " << this->config_.icp_max_err_by_points);
+
+                //update collison msg data.
+                this->collision_msg_.front_icp_err = front_icp_srv_.response.error/front_icp_srv_.response.nvalid;
+                this->collision_msg_.front_laser_disp = front_icp_srv_.response.new_laser_pose;
+                for (unsigned int i=0; i<9; i++)
+                  this->collision_msg_.front_covariance[i] = front_icp_srv_.response.covariance[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)
                 {
                   new_laser_pose << front_icp_srv_.response.new_laser_pose.x, front_icp_srv_.response.new_laser_pose.y, front_icp_srv_.response.new_laser_pose.theta;
@@ -167,13 +187,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                   this->tf2_broadcaster.sendTransform(this->transform_msg);
 
                   this->front_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, front_blink_pose, front_cov, true);
+
+                  this->collision_msg_.front_icp_points = front_icp_srv_.response.nvalid;
                 }
                 else
+                {
+                  this->collision_msg_.front_icp_points = 0;
                   this->front_icp_succedded_ = false;
+                }
               }
               else
               {
                 this->front_icp_succedded_ = false;
+                this->collision_msg_.front_icp_points = 0;
                 ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
               }
               if (this->config_.debug)
@@ -193,13 +219,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
             {
               if (icp_client_.call(rear_icp_srv_))
               {
-                ROS_INFO("CollisionManagerAlgNode::mainNodeThread -> Rear icp_client_ received a response from service server");
-                ROS_INFO_STREAM("   valid " << (int)rear_icp_srv_.response.valid);
-                ROS_INFO_STREAM("   new_laser_pose " << rear_icp_srv_.response.new_laser_pose);
-                ROS_INFO_STREAM("   covariance " << rear_icp_srv_.response.covariance[0] << ", " << rear_icp_srv_.response.covariance[1] << ", " << rear_icp_srv_.response.covariance[2] << ", " << rear_icp_srv_.response.covariance[3] << ", " << rear_icp_srv_.response.covariance[4] << ", " << rear_icp_srv_.response.covariance[5] << ", " << rear_icp_srv_.response.covariance[6] << ", " << rear_icp_srv_.response.covariance[7] << ", " << rear_icp_srv_.response.covariance[8]);
-                ROS_INFO_STREAM("   nvalid " << rear_icp_srv_.response.nvalid << "; min " << this->config_.icp_min_points);
-                ROS_INFO_STREAM("   error " << rear_icp_srv_.response.error);
-                ROS_INFO_STREAM("   error/points " << rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid << "; max " << this->config_.icp_max_err_by_points);
+                // ROS_INFO("CollisionManagerAlgNode::mainNodeThread -> Rear icp_client_ received a response from service server");
+                // ROS_INFO_STREAM("   valid " << (int)rear_icp_srv_.response.valid);
+                // ROS_INFO_STREAM("   new_laser_pose " << rear_icp_srv_.response.new_laser_pose);
+                // ROS_INFO_STREAM("   covariance " << rear_icp_srv_.response.covariance[0] << ", " << rear_icp_srv_.response.covariance[1] << ", " << rear_icp_srv_.response.covariance[2] << ", " << rear_icp_srv_.response.covariance[3] << ", " << rear_icp_srv_.response.covariance[4] << ", " << rear_icp_srv_.response.covariance[5] << ", " << rear_icp_srv_.response.covariance[6] << ", " << rear_icp_srv_.response.covariance[7] << ", " << rear_icp_srv_.response.covariance[8]);
+                // ROS_INFO_STREAM("   nvalid " << rear_icp_srv_.response.nvalid << "; min " << this->config_.icp_min_points);
+                // ROS_INFO_STREAM("   error " << rear_icp_srv_.response.error);
+                // ROS_INFO_STREAM("   error/points " << rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid << "; max " << this->config_.icp_max_err_by_points);
+
+                //update collison msg data.
+                this->collision_msg_.rear_icp_err = rear_icp_srv_.response.error/rear_icp_srv_.response.nvalid;
+                this->collision_msg_.rear_laser_disp = rear_icp_srv_.response.new_laser_pose;
+                for (unsigned int i=0; i<9; i++)
+                  this->collision_msg_.rear_covariance[i] = rear_icp_srv_.response.covariance[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)
                 {
@@ -225,13 +257,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                   this->tf2_broadcaster.sendTransform(this->transform_msg);
 
                   this->rear_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, rear_blink_pose, rear_cov, false);
+
+                  this->collision_msg_.rear_icp_points = rear_icp_srv_.response.nvalid;
                 }
                 else
+                {
+                  this->collision_msg_.rear_icp_points = 0;
                   this->rear_icp_succedded_ = false;
+                }
               }
               else
               {
                 this->rear_icp_succedded_ = false;
+                this->collision_msg_.rear_icp_points = 0;
                 ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
               }
               if (this->config_.debug)
@@ -258,6 +296,9 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           this->low_ang_vel_count_ = 0;
           this->is_front_icp_input_data_ok_ = false;
           this->is_rear_icp_input_data_ok_ = false;
+
+          //publish collision.
+          this->collisions_publisher_.publish(this->collision_msg_);
         }
         else 
         {
@@ -291,6 +332,11 @@ void CollisionManagerAlgNode::mainNodeThread(void)
         {
           this->is_front_icp_input_data_ok_ = set_ref_ranges(this->collision_start_stamp_, true);
           this->is_rear_icp_input_data_ok_ = set_ref_ranges(this->collision_start_stamp_, false);
+
+          if (!this->is_front_icp_input_data_ok_)
+            this->collision_msg_.front_icp_points = 0;
+          if (!this->is_rear_icp_input_data_ok_)
+            this->collision_msg_.rear_icp_points = 0;
         }
 
         //init variables
@@ -299,12 +345,19 @@ void CollisionManagerAlgNode::mainNodeThread(void)
         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;
   }
 
   // [fill msg structures]
+  // Initialize the topic message structure
+  //this->collisions_collision_msg_.data = my_var;
+
   // Initialize the topic message structure
   //this->debug_rear_after_icp_scan_LaserScan_msg_.data = my_var;
 
@@ -392,6 +445,9 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  //this->collisions_publisher_.publish(this->collisions_collision_msg_);
+
   // Uncomment the following line to publish the topic message
   //this->debug_rear_after_icp_scan_publisher_.publish(this->debug_rear_after_icp_scan_LaserScan_msg_);
 
@@ -435,6 +491,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   //this->debug_front_icp_ref_scan_publisher_.publish(this->debug_front_icp_ref_scan_LaserScan_msg_);
 
 
+  this->odom_mutex_exit();
   this->front_laser_scan_mutex_exit();
   this->rear_laser_scan_mutex_exit();
   this->imu_mutex_exit();
@@ -481,16 +538,17 @@ bool CollisionManagerAlgNode::check_acc_collision_transition(double _acc_norm_2d
 
 bool CollisionManagerAlgNode::check_ang_vel_collision_transition(double _ang_vel, bool _start)
 {
+  double diff = std::fabs(_ang_vel - this->angular_vel_);
   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)))
+  if ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < 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)))  
+  return ((_start && (diff > this->config_.collision_ang_vel_th)) || (!_start && (diff < 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))));
 }
 
@@ -498,10 +556,10 @@ bool CollisionManagerAlgNode::check_end_of_collision(double _acc_norm_2d, double
 {
   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);
+  // 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;
 }
 
@@ -509,10 +567,10 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d, doub
 {
   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);
+  // 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;
 }
 
@@ -657,6 +715,31 @@ bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
 }
 
 /*  [subscriber callbacks] */
+void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+  // ROS_INFO("CollisionManagerAlgNode::odom_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  this->odom_mutex_enter();
+  this->angular_vel_ = msg->twist.twist.angular.z;
+
+  //std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  //this->alg_.unlock();
+  this->odom_mutex_exit();
+}
+
+void CollisionManagerAlgNode::odom_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->odom_mutex_);
+}
+
+void CollisionManagerAlgNode::odom_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->odom_mutex_);
+}
+
 void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
 {
   // ROS_INFO("CollisionManagerAlgNode::rear_laser_scan_callback: New Message Received");