Merge branch 'development' of...

Merge branch 'development' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/dogs_project/iri_collision_manager into development

README.md

@@ -8,22 +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
-  - ~**relocalization** (iri_collision_manager/relocalization.msg)
-  - ~**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.
@@ -44,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.
@@ -54,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.
@@ -119,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
@@ -162,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
 
@@ -174,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.
@@ -188,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.
@@ -199,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.