Updated to ICP server

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm tf2_ros sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_laser_scan_icp tf2_ros sensor_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -61,7 +61,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm tf2_ros sensor_msgs
+ CATKIN_DEPENDS iri_base_algorithm 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} iri_laser_scan_icp_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 

README.md

@@ -10,17 +10,27 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 
 # ROS Interface
 ### Topic subscribers
+  - ~**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.
   - ~**imu** (sensor_msgs/Imu.msg): Imu input data.
 
+### Service clients
+  - ~**icp** (iri_laser_scan_icp/icp.srv): ICP server.
+
 ### Parameters
-- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000) The main node thread loop rate in Hz. 
-- ~**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_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.
+- ~**rate** (Double; default: 10.0; min: 0.1; max: 1000): The main node thread loop rate in Hz. 
+- ~**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_timeout** (Double; default: 4.0; min: 1.0; max: 10.0): Timeout to detect an end of collision.
+- ~**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.
+- ~**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.
+- ~**scan_diff_t_from_collision** (Double; default: 0.0; min: 0.0; max: 0.5): Time to subtract from collision time stamp to ensure a clean scan.
 
 ## Installation
 

cfg/CollisionManager.cfg

@@ -39,12 +39,17 @@ 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, 160.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",                                           "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)
 
 
 exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file

config/params.yaml

 rate: 240
+fixed_frame: imu_bno055
 tf_timeout: 0.2
-collision_acc_th: 5.0
+err_msg_rate: 0.5
 collision_transition_counter_en: True
+collision_timeout: 4.0
+collision_acc_th: 5.0
 collision_counter_limit: 12
-err_msg_rate: 0.5
-fixed_frame: imu_bno055
+front_laser_en: True
+rear_laser_en: True
+scan_buffer_size: 6
+scan_diff_t_from_collision: 0.0

include/collision_manager_alg_node.h

@@ -30,14 +30,27 @@
 #include <Eigen/Eigen>
 
 // [publisher subscriber headers]
+#include <sensor_msgs/LaserScan.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
 #include <sensor_msgs/Imu.h>
 
 // [service client headers]
+#include <iri_laser_scan_icp/icp.h>
 
 // [action server client headers]
 
+/**
+ * \struct RangesWithStamp.
+ *
+ * \brief A struct with the ranges and the time stamp info.
+ *
+ */
+typedef struct {
+  std::vector<float> ranges; ///< Laser scan ranges data.
+  ros::Time stamp; ///< Laser scan time stamp.
+}RangesWithStamp;
+
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
@@ -47,17 +60,37 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
   private:
     bool new_imu_input_; ///< Flag to know that new input data has been received.
     Eigen::Vector3d imu_local_acc_; ///< Local input linear acceleration.
+    Eigen::Vector3d imu_local_ang_vel_; ///< Local input angular speed.
+    ros::Time imu_stamp_; ///< IMU stamp.
+    ros::Time last_imu_t_; ///< Stamp from the last imu velocity data integrated.
     std::string imu_frame_; ///< IMU frame id.
     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.
+    // ros::Time collision_end_stamp_; ///< Time stamp at the end of collision.
     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<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.
+    bool is_rear_icp_input_data_ok_; ///<Boolean to know of all the necessary data has been saved on the service request.
 
     // [publisher attributes]
 
     // [subscriber attributes]
+    ros::Subscriber rear_laser_scan_subscriber_;
+    void rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
+    pthread_mutex_t rear_laser_scan_mutex_;
+    void rear_laser_scan_mutex_enter(void);
+    void rear_laser_scan_mutex_exit(void);
+
+    ros::Subscriber front_laser_scan_subscriber_;
+    void front_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
+    pthread_mutex_t front_laser_scan_mutex_;
+    void front_laser_scan_mutex_enter(void);
+    void front_laser_scan_mutex_exit(void);
+
     tf2_ros::Buffer tf2_buffer;
 
     tf2_ros::TransformListener tf2_listener;
@@ -72,6 +105,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     // [service attributes]
 
     // [client attributes]
+    ros::ServiceClient icp_client_;
+    iri_laser_scan_icp::icp front_icp_srv_;
+    iri_laser_scan_icp::icp rear_icp_srv_;
+
 
     // [action server attributes]
 
@@ -103,16 +140,18 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
 
   protected:
     /**
-     * \brief Function to tranform linear local acceleration to fixed frame.
+     * \brief Function to tranform linear local imu data to fixed frame.
      * 
      * 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.
      * 
+     * \param _global_ang_vel The current global angular velocity.
+     * 
      * \return True in success.
      */
-    bool get_global_acc(Eigen::Vector3d& _global_acc);
+    bool get_global_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel);
 
     /**
      * \brief Function to check if is a collision transition.
@@ -150,6 +189,26 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      */
     bool check_start_of_collision(double _acc_norm_2d);
 
+    /**
+     * \brief Function to find the last scan before a collision.
+     * 
+     * \param _stamp The collision time stamp.
+     * 
+     * \param _front To select the laser.
+     * 
+     * \return True if succeeded.
+     */
+    bool set_ref_ranges(ros::Time _stamp, bool _front);
+
+    /**
+     * \brief Function to find the last scan after a collision.
+     * 
+     * \param _front To select the laser.
+     * 
+     * \return True if succeeded.
+     */
+    bool set_last_ranges(bool _front);
+
    /**
     * \brief main node thread
     *

package.xml

@@ -52,6 +52,7 @@
   <build_depend>iri_base_algorithm</build_depend>
   <build_export_depend>iri_base_algorithm</build_export_depend>
   <exec_depend>iri_base_algorithm</exec_depend>
+  <depend>iri_laser_scan_icp</depend>
   <depend>tf2_ros</depend>
   <depend>sensor_msgs</depend>
   <depend>iri_bno055_imu_bringup</depend>

src/collision_manager_alg_node.cpp

@@ -23,10 +23,17 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   this->tf_loaded_ = false;
   this->in_collision_ = false;
   this->low_acc_count_ = 0;
-
+  this->is_front_icp_input_data_ok_ = false;
+  this->is_rear_icp_input_data_ok_ = false;
   // [init publishers]
   
   // [init subscribers]
+  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);
+
+  this->front_laser_scan_subscriber_ = this->private_node_handle_.subscribe("front_laser_scan", 1, &CollisionManagerAlgNode::front_laser_scan_callback, this);
+  pthread_mutex_init(&this->front_laser_scan_mutex_,NULL);
+
   this->imu_subscriber_ = this->private_node_handle_.subscribe("imu", 1, &CollisionManagerAlgNode::imu_callback, this);
   pthread_mutex_init(&this->imu_mutex_,NULL);
 
@@ -34,6 +41,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
   // [init services]
   
   // [init clients]
+  icp_client_ = this->private_node_handle_.serviceClient<iri_laser_scan_icp::icp>("icp");
+
   
   // [init action servers]
   
@@ -43,6 +52,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
 CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
 {
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->rear_laser_scan_mutex_);
+  pthread_mutex_destroy(&this->front_laser_scan_mutex_);
   pthread_mutex_destroy(&this->imu_mutex_);
 }
 
@@ -52,11 +63,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   // this->alg_.lock();
   // ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread");
   this->imu_mutex_enter();
+  this->rear_laser_scan_mutex_enter();
+  this->front_laser_scan_mutex_enter();
 
   if (this->new_imu_input_)
   {
-    Eigen::Vector3d global_acc;
-    if (get_global_acc(global_acc))
+    Eigen::Vector3d global_acc, global_ang_vel;
+    if (get_global_imu_data(global_acc, global_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));
       if (this->in_collision_)
@@ -71,17 +84,90 @@ void CollisionManagerAlgNode::mainNodeThread(void)
             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);
+
+          //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->rear_icp_srv_.request.rot_angle_estimation = -this->front_icp_srv_.request.rot_angle_estimation;
+
+          // Call the service
+          if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
+            ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Collision end detected but no laser is enabled.");
+          else
+          {
+            if (this->is_front_icp_input_data_ok_ && set_last_ranges(true))
+            {
+              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);
+               ROS_INFO_STREAM("   error " << front_icp_srv_.response.error);
+              }
+              else
+               ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
+            }
+            if (this->is_rear_icp_input_data_ok_ && set_last_ranges(false))
+            {
+              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);
+               ROS_INFO_STREAM("   error " << rear_icp_srv_.response.error);
+              }
+              else
+               ROS_ERROR("CollisionManagerAlgNode::mainNodeThread -> Failed to call service on topic %s",this->icp_client_.getService().c_str());
+            }
+          }
+          //restart variables
           this->in_collision_ = false;
           this->low_acc_count_ = 0;
+          this->is_front_icp_input_data_ok_ = false;
+          this->is_rear_icp_input_data_ok_ = false;
+        }
+        else 
+        {
+          //Update max_acc
+          if (acc_norm_2d > max_acc_norm_2d)
+            this->max_acc_ = global_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->last_imu_t_ = this->imu_stamp_;
+
+          //Check collision timeout
+          if ((ros::Time::now() - this->collision_start_stamp_) > ros::Duration(this->config_.collision_timeout))
+          {
+            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;
+          }
         }
-        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->collision_start_stamp_ = this->imu_stamp_;
+
+        // Get reference scan
+        if (!(this->config_.front_laser_en || this->config_.rear_laser_en))
+          ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Collision detected but no laser is enabled.");
+        else
+        {
+          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);
+        }
+
+        //init variables
         this->in_collision_ = true;
         this->max_acc_ = global_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;
       }
     }
     this->new_imu_input_ = false;
@@ -134,23 +220,38 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
   
   // [fill srv structure and make request to the server]
+  //icp_srv_.request.data = my_var;
+  //ROS_INFO("CollisionManagerAlgNode:: Calling service icp_client_!");
+  //if (icp_client_.call(icp_srv_))
+  //{
+  //  ROS_INFO("CollisionManagerAlgNode:: icp_client_ received a response from service server");
+  ////  ROS_INFO("CollisionManagerAlgNode:: Response: %s", icp_srv_.response.somestringfield.c_str());
+  //}
+  //else
+  //{
+  //  ROS_INFO("CollisionManagerAlgNode:: Failed to call service on topic %s",this->icp_client_.getService().c_str());
+  //}
+
+
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
 
+  this->front_laser_scan_mutex_exit();
+  this->rear_laser_scan_mutex_exit();
   this->imu_mutex_exit();
   // this->alg_.unlock();
 }
 
-bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc)
+bool CollisionManagerAlgNode::get_global_imu_data(Eigen::Vector3d& _global_acc, Eigen::Vector3d& _global_ang_vel)
 {
   if (this->config_.fixed_frame != this->imu_frame_ || !this->tf_loaded_)
   {
     geometry_msgs::TransformStamped tf_fixed_frame_imu_msg;
     if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
     {
-      ROS_WARN_STREAM("CollisionManagerAlgNode::get_global_acc -> Can't transform from " << this->config_.fixed_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
+      ROS_WARN_STREAM("CollisionManagerAlgNode::get_global_imu_data -> Can't transform from " << this->config_.fixed_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
       this->tf_loaded_ = false;
       return false;
     }
@@ -162,6 +263,7 @@ bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc)
     this->tf_loaded_ = true;
   }
   _global_acc = this->tf_fixed_frame_imu_*this->imu_local_acc_;
+  _global_ang_vel = this->tf_fixed_frame_imu_*this->imu_local_ang_vel_;
   return true;
 }
 
@@ -191,7 +293,121 @@ bool CollisionManagerAlgNode::check_start_of_collision(double _acc_norm_2d)
   return check_collision_transition(_acc_norm_2d, true);
 }
 
+bool CollisionManagerAlgNode::set_ref_ranges(ros::Time _stamp, bool _front)
+{
+  if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
+    return false;
+  auto it = (_front? this->front_ranges_.begin(): this->rear_ranges_.begin());
+  while ((it != (_front? this->front_ranges_.end(): this->rear_ranges_.end())) && (it->stamp > (this->collision_start_stamp_ - ros::Duration(this->config_.scan_diff_t_from_collision))))
+    ++it;
+  if (it == (_front? this->front_ranges_.end(): this->rear_ranges_.end()))
+  {
+    ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> Collision detected at " << this->collision_start_stamp_ << " but there is no " 
+                      << (_front? "front ": "rear ") << "scan before collision.");
+    if ((_front? this->front_ranges_: this->rear_ranges_).size() > 0)
+      ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> scan times: " << (_front? this->front_ranges_: this->rear_ranges_).front().stamp << ", " << (_front? this->front_ranges_: this->rear_ranges_).back().stamp);
+    else
+      ROS_WARN_STREAM("CollisionManagerAlgNode::set_ref_ranges -> No scan buffered");
+    return false;
+  }
+  (_front? this->front_icp_srv_: this->rear_icp_srv_).request.ref_ranges = it->ranges;
+  return true;
+}
+
+bool CollisionManagerAlgNode::set_last_ranges(bool _front)
+{
+  if (!(_front? this->config_.front_laser_en: this->config_.rear_laser_en))
+    return false;
+
+  if ((_front? this->front_ranges_: this->rear_ranges_).size() > 0)
+  {
+    (_front? this->front_icp_srv_: this->rear_icp_srv_).request.last_ranges = (_front? this->front_ranges_: this->rear_ranges_).front().ranges;
+    return true;
+  }
+  ROS_WARN_STREAM("CollisionManagerAlgNode::set_last_ranges -> No scan buffered");
+  return false;
+}
+
 /*  [subscriber callbacks] */
+void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+  // ROS_INFO("CollisionManagerAlgNode::rear_laser_scan_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  this->rear_laser_scan_mutex_enter();
+  if (this->config_.rear_laser_en)
+  {
+    rear_icp_srv_.request.angle_min = msg->angle_min;
+    rear_icp_srv_.request.angle_max = msg->angle_max;
+    rear_icp_srv_.request.angle_increment = msg->angle_increment;
+    rear_icp_srv_.request.scan_time = msg->scan_time;
+    rear_icp_srv_.request.range_min = msg->range_min;
+    rear_icp_srv_.request.range_max = msg->range_max;
+
+    RangesWithStamp r;
+    r.ranges = msg->ranges;
+    r.stamp = msg->header.stamp;
+    if (this->rear_ranges_.size() >= this->config_.scan_buffer_size)
+      this->rear_ranges_.pop_back();
+    this->rear_ranges_.push_front(r);
+  }
+  //std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  //this->alg_.unlock();
+  this->rear_laser_scan_mutex_exit();
+}
+
+void CollisionManagerAlgNode::rear_laser_scan_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->rear_laser_scan_mutex_);
+}
+
+void CollisionManagerAlgNode::rear_laser_scan_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->rear_laser_scan_mutex_);
+}
+
+void CollisionManagerAlgNode::front_laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+  // ROS_INFO("CollisionManagerAlgNode::front_laser_scan_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  this->front_laser_scan_mutex_enter();
+  if (this->config_.front_laser_en)
+  {
+    front_icp_srv_.request.angle_min = msg->angle_min;
+    front_icp_srv_.request.angle_max = msg->angle_max;
+    front_icp_srv_.request.angle_increment = msg->angle_increment;
+    front_icp_srv_.request.scan_time = msg->scan_time;
+    front_icp_srv_.request.range_min = msg->range_min;
+    front_icp_srv_.request.range_max = msg->range_max;
+
+    RangesWithStamp r;
+    r.ranges = msg->ranges;
+    r.stamp = msg->header.stamp;
+    if (this->front_ranges_.size() >= this->config_.scan_buffer_size)
+      this->front_ranges_.pop_back();
+    this->front_ranges_.push_front(r);
+  }
+
+  //std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  //this->alg_.unlock();
+  this->front_laser_scan_mutex_exit();
+}
+
+void CollisionManagerAlgNode::front_laser_scan_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->front_laser_scan_mutex_);
+}
+
+void CollisionManagerAlgNode::front_laser_scan_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->front_laser_scan_mutex_);
+}
+
 void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
 {
   // ROS_INFO("CollisionManagerAlgNode::imu_callback: New Message Received");
@@ -202,8 +418,12 @@ 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;
+  this->imu_local_ang_vel_ = Eigen::Vector3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
+  this->imu_stamp_ = msg->header.stamp;
+  // if (!this->in_collision_)
+  //   this->collision_start_stamp_ = msg->header.stamp;
+  // if (this->in_collision_)
+  //   this->collision_end_stamp_ = msg->header.stamp;
   //std::cout << msg->data << std::endl;
   //unlock previously blocked shared variables
   //this->alg_.unlock();
@@ -230,9 +450,17 @@ void CollisionManagerAlgNode::imu_mutex_exit(void)
 void CollisionManagerAlgNode::node_config_update(Config &config, uint32_t level)
 {
   this->alg_.lock();
+  this->imu_mutex_enter();
+  this->rear_laser_scan_mutex_enter();
+  this->front_laser_scan_mutex_enter();
+
   if(config.rate!=this->getRate())
     this->setRate(config.rate);
   this->config_=config;
+
+  this->front_laser_scan_mutex_exit();
+  this->rear_laser_scan_mutex_exit();
+  this->imu_mutex_exit();
   this->alg_.unlock();
 }