Added general diagnostic agregator

README.md

@@ -198,6 +198,7 @@ This node provides the following launch files:
   - *launch_prefix*: Ros node launch launch-prefix's parameter.
   - *dr*: Boolean to launch rqt_reconfigure.
   - *diagnostics*: Flag to launch diagnostics agregator.
+  - *diagnostics_agregator_config_file*: Diagnostic agregator config file.
   - *sim_time*: Boolean to set */use_sim_time* parameter.
   - *imu_driver*: Boolean to launch the IMU driver.
 

config/collision_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']

launch/collisions.launch

@@ -8,6 +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" />
 
   <!--  IMU driver parameters  -->
   <arg name="imu_config_file"      default="$(find iri_collision_manager)/config/bno055.yaml" />
@@ -47,7 +48,7 @@
     <arg name="imu_topic_out" value="$(arg imu_filtered_topic)"/>
     <arg name="odom_topic" value="$(arg odom_topic)"/>
     <arg name="update_bias_ns" value="$(arg update_bias_ns)"/>
-    <arg name="diagnostics" value="$(arg diagnostics)"/>
+    <arg name="diagnostics" value="false"/>
   </include>
 
   <include file="$(find iri_laser_scan_icp)/launch/node.launch">
@@ -68,7 +69,7 @@
     <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)"/>
-    <arg name="diagnostics" value="$(arg diagnostics)"/>
+    <arg name="diagnostics" value="false"/>
   </include>
 
 
@@ -79,4 +80,13 @@
         args="iri_collision_manager">
   </node>
 
+  <node pkg="diagnostic_aggregator" 
+        type="aggregator_node"
+        name="collision_diagnostic_aggregator" 
+        if  ="$(arg diagnostics)">
+    <!-- Load the file you made above -->
+    <rosparam command="load" 
+              file="$(find iri_collision_manager)/config/collision_analyzers.yaml" />
+  </node>
+
 </launch>