CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(Eigen3 REQUIRED)
 add_message_files(
   FILES
   collision.msg
+  relocalization.msg
 )
 
 ## Generate services in the 'srv' folder

README.md

@@ -8,21 +8,27 @@ There are two possible collision's triggers:
 
 When a collision detection is triggered, an ICP for each laser is performed. When an ICP has been succesful (meaning that the points used for the calculation are more than *icp_min_points* and the error per point is lees than *icp_max_err_by_points*) its odometry is compared with the wheel odometry. If the difference between both is big enough (meaning that is bigger than *odom_trans_th* or *odom_angular_th*), is considered as a collision. The node that performs that ICP is [iri_laser_scan_icp](https://gitlab.iri.upc.edu/mobile_robotics/dogs_project/iri_laser_scan_icp).
 
+A ROS [laser scan range filter](http://wiki.ros.org/laser_filters) is performed on laser scans to remove the closest points (the object that performs the collision and is a mobile object) before the ICP.
+
 This node also provides the following configuration files located in [config](./config) directory:
   * **params.yaml**: Node default configuration file.
   * **bno055.yaml**: IMU default configuration file.
   * **analyzers.yaml**: Diagnostics aggregator's default configuration file.
+  * **collision_analyzers.yaml**: Diagnostics aggregator's for collisions configuration file.
+  * **dogs_analyzers.yaml**: Diagnostics aggregator's for all the system configuration file.
+  * **range_filter.yaml**: To configure laser scan range filter.
 
 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)
+  - ~**relocalization** (iri_collision_manager/relocalization.msg): Topic to publish the ICP relocalization info.
+  - ~**collisions** (iri_collision_manager/collision.msg): Topic to publish Collision info.
+  - ~**debug_rear_after_icp_scan** (sensor_msgs/LaserScan.msg): Debug topic to visualize rear laser after ICP:
+  - ~**debug_front_after_icp_scan** (sensor_msgs/LaserScan.msg): Debug topic to visualize front laser after ICP:
   - /**tf** (tf/tfMessage)
-  - ~**debug_rear_icp_pose** (geometry_msgs/PoseWithCovariance.msg)
-  - ~**debug_front_icp_pose** (geometry_msgs/PoseWithCovariance.msg)
+  - ~**debug_rear_icp_pose** (geometry_msgs/PoseWithCovariance.msg): Debug topic to visualize robot's pose after rear ICP:
+  - ~**debug_front_icp_pose** (geometry_msgs/PoseWithCovariance.msg): Debug topic to visualize robot's pose after front ICP:
   - ~**debug_rear_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize rear ICP matching laser scan.
   - ~**debug_rear_icp_ref_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize rear ICP reference laser scan.
   - ~**debug_front_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize front ICP matching laser scan.
@@ -43,7 +49,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 - ~**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: map): Fixed frame id.
-- ~**fixed_frame** (String; default: base_link): Base link frame id.
+- ~**base_link_frame** (String; default: base_link): Base link 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.
 - ~**calculate_ang_vel** (Boolean; default: False): Flag to calculate robot angular vel from odometry.
@@ -53,9 +59,11 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 - ~**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 for accelerometer detections. 
 - ~**collision_acc_th** (Double; default: 9.8; min: 0.1; max: 160.0): Acceleration threshold to detect a collision.
+- ~**collision_acc_th_end** (Double; default: 9.8; min: 0.1; max: 160.0): Acceleration threshold 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.
 - ~**collision_ang_vel_transition_counter_en** (Boolean; default: False): Enable the collision counter for angular velocity detections. 
 - ~**collision_ang_vel_th** (Double; default: 0.3; min: 0.1; max: 1.0): Angular velocity threshold to detect a collision.
+- ~**collision_ang_vel_th_end** (Double; default: 0.3; min: 0.1; max: 1.0): Angular velocity threshold to detect an end of collision.
 - ~**collision_ang_vel_counter_limit** (Integer; default: 6; min: 1; max: 30): Number of low acc imu meassures to detect an end of collision.
 - ~**odom_trans_th** (Double; default: 0.1; min: 0.01; max: 0.5): Max translational difference between wheels and ICP odometries.
 - ~**odom_angular_th** (Double; default: 0.154¡; min: 0.01; max: 0.7): Max angular difference between wheels and ICP odometries in rad.
@@ -118,7 +126,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
   sudo ldconfig
   ```
 
-  At this point we will have installed all the c++ libraries. /etc/ must have the following lines:
+  At this point we will have installed all the c++ libraries. */etc/ld.so.conf.d/iri.conf* must have the following lines:
   ```
   /usr/local/lib/iri/iriutils
   /usr/local/lib/iri/comm
@@ -161,6 +169,11 @@ Compile the workspace:
 ```
 catkin_make
 ```
+## Diagnostics
+
+This node has the following diagnostics:
+* A HeartBeat to know that the node is alive.
+* A subscriber diagnostic to check that imu, lasers and odometry data is received with the desired frequency.
 
 ## How to use it
 
@@ -173,6 +186,7 @@ This node provides the following launch files:
   - *odom_topic*: Odometry input data topic name to be remaped.
   - *front_laser_topic*: Front laser input data topic name to be remaped.
   - *rear_laser_topic*: Rear laser input data topic name to be remaped.
+  - *relocalization_topic_name*: relocalization output topic name to be remaped.
   - *icp_service_name*: Laser_scan ICP service to be remaped.
   - *diagnostics*: Flag to launch diagnostics agregator.
   - *output*: Ros node launch output's parameter.
@@ -187,7 +201,7 @@ This node provides the following launch files:
   - *diagnostics*: Flag to launch diagnostics agregator.
   - *sim_time*: Boolean to set */use_sim_time* parameter.
 
-- **collisions.launch**: General launch file with IMU driver, IMU bias filter, laser_scan ICP and collision managment. It has the following arguments:
+- **collisions.launch**: General launch file with IMU driver, IMU bias filter,laser scan range filters, laser_scan ICP and collision managment. It has the following arguments:
   - *imu_config_file*: IMU driver node configuration yaml file.
   - *imu_calibration_file*: IMU calibration file (not used for raw data mode).
   - *bias_filter_config_file*: IMU bias filter configuration yaml file.
@@ -198,8 +212,12 @@ This node provides the following launch files:
   - *icp_config_file*: Laser_scan ICP configuration yaml file.
   - *icp_service_name*: Laser_scan ICP service to be remaped.
   - *collision_config_file*: Collision managment node configuration yaml file.
+  - *relocalization_topic_name*: relocalization output topic name to be remaped.
+  - *filter_config_file*: Laser Scan Range Filter configuration yaml file.
   - *front_laser_topic*: Front laser input data topic name to be remaped.
+  - *front_laser_topic_filtered*: Filtered front laser output data topic name to be remaped.
   - *rear_laser_topic*: Rear laser input data topic name to be remaped.
+  - *rear_laser_topic_filtered*: Filtered rear laser output data topic name to be remaped.
   - *output*: Ros node launch output's parameter.
   - *launch_prefix*: Ros node launch launch-prefix's parameter.
   - *dr*: Boolean to launch rqt_reconfigure.

cfg/CollisionManager.cfg

@@ -52,14 +52,16 @@ gen.add("watchdog_t",                           double_t,   0,           "Maximu
 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)
 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_th_end",                     double_t,   0,           "Acceleration threshold to detect a end of 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_th_end",                 double_t,   0,           "Angular velocity threshold to detect a end of 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)
 collision_detection.add("odom_trans_th",                            double_t,   0,           "Max translational difference between wheels and ICP odometries",   0.1,    0.01, 0.5)
 collision_detection.add("odom_angular_th",                          double_t,   0,           "Max angular difference between wheels and ICP odometries in rad",  0.15,   0.05, 0.7)
 
-icp.add("buffer_size",                          int_t,      0,           "Number of laser_scan and odometry buffered for the ICP",              3, 1, 30)
+icp.add("buffer_size",                          int_t,      0,           "Number of laser_scan and odometry buffered for the ICP",              3, 1, 50)
 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)

config/dogs_analyzers.yaml

+pub_rate: 1.0
+analyzers:
+  collision_detection:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: collision_detection
+    contains: [
+      'imu',
+      'collision',
+      'odom']
+  ICP_check:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: icp_check
+    contains: [
+      'odom',
+      'laser']
+  bias_filter:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: bias_filter
+    contains: [
+      'bias']
+  localization_fusion:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: localization_fusion
+    contains: [
+      'AMCL',
+      'lost',
+      'pose',
+      'localized']
+  landmarks_tracker:
+    type: diagnostic_aggregator/GenericAnalyzer
+    path: landmarks_tracker
+    contains: [
+      'scan',
+      'odom']

config/params.yaml

-rate: 120
+rate: 150
 debug: True
 fixed_frame: map
 base_link_frame: base_link
 tf_timeout: 0.2
 err_msg_rate: 0.5
-calculate_ang_vel: True
+calculate_ang_vel: False
 watchdog_t: 1.0
 
-collision_timeout: 4.0
+collision_timeout: 8.0
 collision_acc_transition_counter_en: True
 collision_acc_th: 5.0
-collision_acc_counter_limit: 20
+collision_acc_th_end: 2.0
+collision_acc_counter_limit: 10
 collision_ang_vel_transition_counter_en: True
-collision_ang_vel_th: 0.3
+collision_ang_vel_th: 0.4
+collision_ang_vel_th_end: 0.2
 #collision_ang_vel_th: 0.2
-collision_ang_vel_counter_limit: 20
+collision_ang_vel_counter_limit: 10
 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.1
+buffer_size: 30
+scan_diff_t_from_collision: 0.0
 front_laser_en: True
 rear_laser_en: True
-icp_min_points: 260
-icp_max_err_by_points: 0.4
+icp_min_points: 80
+icp_max_err_by_points: 0.3

config/range_filter.yaml

+scan_filter_chain:
+- name: range
+  type: laser_filters/LaserScanRangeFilter
+  params:
+    use_message_range_limits: false
+    lower_threshold: 3.5
+    upper_threshold: .inf
+    lower_replacement_value: .inf
+    upper_replacement_value: .inf

include/collision_manager_alg_node.h

@@ -32,6 +32,7 @@
 #include <iri_ros_tools/watchdog.h>
 
 // [publisher subscriber headers]
+#include <iri_collision_manager/relocalization.h>
 #include <iri_collision_manager/collision.h>
 #include <nav_msgs/Odometry.h>
 #include <tf2_ros/transform_broadcaster.h>
@@ -72,6 +73,18 @@ typedef struct {
   ros::Time stamp; ///< Time stamp.
 }OdomWithStamp;
 
+/**
+ * \struct ImuWithStamp.
+ *
+ * \brief A struct with the IMU information and the time stamp.
+ *
+ */
+typedef struct {
+  Eigen::Vector3d acc; ///< IMU accelerometer data.
+  Eigen::Vector3d ang_vel; ///< IMU angular velocity data.
+  ros::Time stamp; ///< Time stamp.
+}ImuWithStamp;
+
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
@@ -82,6 +95,7 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     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.
+    std::list<ImuWithStamp> imu_buffer_; ///< IMU data buffer.
     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.
@@ -90,10 +104,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     bool tf_base_link_imu_loaded_; ///< Flag to know that tf_base_link_imu has been loaded before.
     Eigen::Transform<double,3,Eigen::Affine> tf_base_link_imu_; ///< Transform from base_link frame to IMU frame.
     bool tf_base_link_front_laser_loaded_; ///< Flag to know that tf_base_link_front_laser has been loaded before.
-    tf2::Transform tf_blink_front_laser; ///< Transform from base_link to front laser.
+    tf2::Transform tf_blink_front_laser_; ///< Transform from base_link to front laser.
     bool z_axis_blink_front_laser_inv_; ///< Flag to know if the z axis of base link and front laser have the same orientation.
     bool tf_base_link_rear_laser_loaded_; ///< Flag to know that tf_base_link_rear_laser has been loaded before.
-    tf2::Transform tf_blink_rear_laser; ///< Transform from base_link to rear laser.
+    tf2::Transform tf_blink_rear_laser_; ///< Transform from base_link to rear laser.
     bool z_axis_blink_rear_laser_inv_; ///< Flag to know if the z axis of base link and rear laser have the same orientation.
     bool in_collision_; ///< Flag to know that we are receiving collision meassures.
     ros::Time collision_start_stamp_; ///< Time stamp of a collision initial detection.
@@ -118,6 +132,9 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     CROSWatchdog rear_laser_watchdog_; ///< Rear laser callback watchdog.
 
     // [publisher attributes]
+    ros::Publisher relocalization_publisher_;
+    iri_collision_manager::relocalization relocalization_msg_;
+
     ros::Publisher collisions_publisher_;
     iri_collision_manager::collision collision_msg_;
 
@@ -230,18 +247,35 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
 
   protected:
     /**
-     * \brief Function to tranform linear local imu data to base_link frame.
+     * \brief Function to tranform the current local imu data to base_link frame.
+     * 
+     * \param _blink_acc The current global linear acceleration.
+     * 
+     * \param _blink_ang_vel The current global angular velocity.
+     * 
+     * \return True in success.
+     */
+    bool get_current_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
+
+    /**
+     * \brief Function to tranform any local imu data to base_link frame.
      * 
      * It checks if imu frame id is different from base_link frame and if not already 
      * saved, it saves the transfrom.
      * 
-     * \param _blink_acc The current global linear acceleration.
+     * \param _local_acc The local linear acceleration.
      * 
-     * \param _blink_ang_vel The current global angular velocity.
+     * \param _local_ang_vel The local angular velocity.
+     * 
+     * \param _stamp The target stamp.
+     *  
+     * \param _blink_acc The global linear acceleration.
+     * 
+     * \param _blink_ang_vel The global angular velocity.
      * 
      * \return True in success.
      */
-    bool get_base_link_imu_data(Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
+    bool transform_to_base_link_imu_data(const Eigen::Vector3d& _local_acc, const Eigen::Vector3d& _local_ang_vel, const ros::Time& _stamp, Eigen::Vector3d& _blink_acc, Eigen::Vector3d& _blink_ang_vel);
 
     /**
      * \brief Function to check if is an acceleration collision transition.
@@ -298,6 +332,20 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      */
     bool check_start_of_collision(double _acc_norm_2d, double _ang_vel);
 
+    /**
+     * \brief Function to find the stamp of the first imu data that triggers the collision.
+     * 
+     * \return True if succeeded.
+     */
+    bool set_collision_start(void);
+
+    /**
+     * \brief Function to calculate the angle rotated while detecting a collision.
+     * 
+     * \return The angle calculated.
+     */
+    double set_initial_angle_rotated(void);
+
     /**
      * \brief Function to find the last scan before a collision.
      * 
@@ -367,17 +415,21 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      * 
      * \param _covariances Laser pose covariances from ICP.
      * 
-     * \param _odom The 2D base link pose.
+     * \param _odom The 2D base link pose on odom frames.
+     * 
+     * \param _odom_cov Its covariances on odom frames.
+     * 
+     * \param _odom_diff The ICP displacement on odom frame.
      * 
-     * \param _odom_cov Its covariances.
+     * \param _odom_diff_blink The ICP displacement on blink frame.
      * 
-     * \param _odom_diff The ICP displacment on odom frame.
+     * \param _odom_blink_cov Its covariances on blink frame.
      * 
      * \param _front To select the laser.
      * 
      * \return True if succeeded.
      */
-    bool 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);
+    bool get_icp_odometry(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _odom, Eigen::Matrix3d& _odom_cov, Eigen::Vector3d& _odom_diff, Eigen::Vector3d& _odom_diff_blink, Eigen::Matrix3d& _odom_blink_cov, const bool _front);
 
     /**
      * \brief Function to compare the odometry with the ICP to check that is actually a collision.
@@ -390,6 +442,25 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      */
     bool check_odometry_diff(const Eigen::Vector3d& _icp_odometry, Eigen::Vector3d& _icp_odometry_diff);
 
+    /**
+     * \brief Function to get the new robot's base_link pose from ICP result.
+     * 
+     * It checks that ICP result is good enough and calculates the pose.
+     * 
+     * \param _icp_odom_diff ICP differential odometry in odom frame.
+     * 
+     * \param _icp_covariances Laser pose covariances from ICP.
+     * 
+     * \param _blink_icp ICP on base link frame.
+     * 
+     * \param _blink_icp_cov ICP covariances on base link frame.
+     * 
+     * \param _front To select the laser.
+     * 
+     * \return True if succeeded.
+     */
+    // bool icp_odometry_to_blink(const Eigen::Vector3d& _icp_odom_diff, const Eigen::Matrix3d& _icp_covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front);
+
    /**
     * \brief main node thread
     *

launch/collisions.launch

@@ -8,7 +8,7 @@
   <param name="/use_sim_time" value="$(arg sim_time)" />
   <arg name="imu_driver"    default="true"/>
   <arg name="diagnostics"   default="true"/>
-  <arg name="diagnostics_agregator_config_file"      default="$(find iri_collision_manager)/config/collision_analyzers.yaml" />
+  <arg name="diagnostics_agregator_config_file"      default="$(find iri_collision_manager)/config/dogs_analyzers.yaml" />
 
   <!--  IMU driver parameters  -->
   <arg name="imu_config_file"      default="$(find iri_collision_manager)/config/bno055.yaml" />
@@ -27,8 +27,14 @@
 
   <!--  Collision manager parameters  -->
   <arg name="collision_config_file"   default="$(find iri_collision_manager)/config/params.yaml"/>
+  <arg name="relocalization_topic_name"  default="~/icp_relocalization"/>
+
+  <!--  Laser filters parameters -->
+  <arg name="filter_config_file"   default="$(find iri_collision_manager)/config/range_filter.yaml"/>
   <arg name="front_laser_topic"       default="/laser_front/scan"/>
   <arg name="rear_laser_topic"        default="/laser_rear/scan"/>
+  <arg name="front_laser_topic_filtered"       default="/laser_front/scan_filtered"/>
+  <arg name="rear_laser_topic_filtered"        default="/laser_rear/scan_filtered"/>
 
   <include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
            if="$(arg imu_driver)">
@@ -65,13 +71,29 @@
     <arg name="launch_prefix" value="$(arg launch_prefix)"/>
     <arg name="config_file" value="$(arg collision_config_file)"/>
     <arg name="imu_topic" value="$(arg imu_filtered_topic)"/>
-    <arg name="front_laser_topic" value="$(arg front_laser_topic)"/>
-    <arg name="rear_laser_topic" value="$(arg rear_laser_topic)"/>
+    <arg name="front_laser_topic" value="$(arg front_laser_topic_filtered)"/>
+    <arg name="rear_laser_topic" value="$(arg rear_laser_topic_filtered)"/>
     <arg name="icp_service_name" value="$(arg icp_service_name)"/>
     <arg name="odom_topic" value="$(arg odom_topic)"/>
+    <arg name="relocalization_topic_name" value="$(arg relocalization_topic_name)"/>
     <arg name="diagnostics" value="false"/>
   </include>
 
+  <node pkg="laser_filters" 
+        type="scan_to_scan_filter_chain"
+        name="front_laser_filter">
+    <rosparam command="load" file="$(arg filter_config_file)" />
+    <remap from="/scan" to="$(arg front_laser_topic)" />
+    <remap from="/scan_filtered" to="$(arg front_laser_topic_filtered)" />
+  </node>
+
+  <node pkg="laser_filters" 
+        type="scan_to_scan_filter_chain"
+        name="rear_laser_filter">
+    <rosparam command="load" file="$(arg filter_config_file)" />
+    <remap from="/scan" to="$(arg rear_laser_topic)" />
+    <remap from="/scan_filtered" to="$(arg rear_laser_topic_filtered)" />
+  </node>
 
   <node name="rqt_reconfigure_iri_collision_manager"
         pkg ="rqt_reconfigure"
@@ -86,7 +108,7 @@
         if  ="$(arg diagnostics)">
     <!-- Load the file you made above -->
     <rosparam command="load" 
-              file="$(find iri_collision_manager)/config/collision_analyzers.yaml" />
+              file="$(arg diagnostics_agregator_config_file)" />
   </node>
 
 </launch>

launch/node.launch

@@ -13,6 +13,7 @@
   <arg name="rear_laser_topic"     default="~/rear_laser_scan"/>
   <arg name="icp_service_name"        default="~/icp"/>
   <arg name="odom_topic"     default="~/odom"/>
+  <arg name="relocalization_topic_name"  default="~/relocalization"/>
 
   <node name="$(arg node_name)"
         pkg ="iri_collision_manager"
@@ -26,6 +27,7 @@
     <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)"/>
+    <remap from="~/relocalization" to="$(arg relocalization_topic_name)"/>
   </node>
 
   <node pkg="diagnostic_aggregator" 

msg/relocalization.msg

+Header header                           # Msg header.
+
+bool front_icp_check                    # If front ICP is enough good to relocalization.
+geometry_msgs/Pose2D front_icp_odom     # Front laser icp odometry in base_link reference.
+float32[9] front_odom_covariance        # Front ICP odometry covariance.
+geometry_msgs/Pose2D front_blink_pose   # Robot pose from front ICP.
+float32[9] front_pose_covariance        # Robot pose covariance.
+
+bool rear_icp_check                    # If rear ICP is enough good to relocalization.
+geometry_msgs/Pose2D rear_icp_odom     # Rear laser icp odometry in base_link reference.
+float32[9] rear_odom_covariance        # Rear ICP odometry covariance.
+geometry_msgs/Pose2D rear_blink_pose   # Robot pose from rear ICP.
+float32[9] rear_pose_covariance        # Robot pose covariance.

package.xml

@@ -54,6 +54,7 @@
   <build_depend>message_generation</build_depend>
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>iri_base_algorithm</exec_depend>
+  <exec_depend>laser_filters</exec_depend>
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>iri_laser_scan_icp</depend>