From 56f3b51971029dca9cb4b51d269c7041c6117f94 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Thu, 12 May 2022 00:25:38 +0900
Subject: [PATCH 1/5] Revert "Test /head_camera/rgb/image_raw"

This reverts commit a5f30540811e7fec7ca0788215441d08c30a1c15.
---
 .../jsk_fetch_startup/config/test_device_topics.yaml     | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml b/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml
index 3ff93202dc..a52b7dcda2 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml
+++ b/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml
@@ -19,14 +19,11 @@ timeout_5: 10
 topic_6: /head_camera/depth/image_raw
 timeout_6: 10
 
-topic_7: /head_camera/rgb/image_raw
+topic_7: /imu
 timeout_7: 10
 
-topic_8: /imu
+topic_8: /insta360/image_raw
 timeout_8: 10
 
-topic_9: /insta360/image_raw
+topic_9: /tf
 timeout_9: 10
-
-topic_10: /tf
-timeout_10: 10

From 17e1e4aa0edc9292add65294191afb55d8893c27 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Thu, 12 May 2022 00:25:45 +0900
Subject: [PATCH 2/5] Revert "Check if the ROS topics using the fetch device
 are published."

This reverts commit 183198706901ffab029ad43c4b964ca21f31c6d5.
---
 .../config/test_device_topics.yaml            | 29 -------------------
 .../launch/fetch_test_topics.launch           |  9 ------
 2 files changed, 38 deletions(-)
 delete mode 100644 jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml
 delete mode 100644 jsk_fetch_robot/jsk_fetch_startup/launch/fetch_test_topics.launch

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml b/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml
deleted file mode 100644
index a52b7dcda2..0000000000
--- a/jsk_fetch_robot/jsk_fetch_startup/config/test_device_topics.yaml
+++ /dev/null
@@ -1,29 +0,0 @@
-topic_0: /audio
-timeout_0: 10
-
-topic_1: /base_scan
-timeout_1: 10
-
-topic_2: /battery_state
-timeout_2: 10
-
-topic_3: /edgetpu_human_pose_estimator/output/image
-timeout_3: 10
-
-topic_4: /edgetpu_object_detector/output/image
-timeout_4: 10
-
-topic_5: /gripper/imu
-timeout_5: 10
-
-topic_6: /head_camera/depth/image_raw
-timeout_6: 10
-
-topic_7: /imu
-timeout_7: 10
-
-topic_8: /insta360/image_raw
-timeout_8: 10
-
-topic_9: /tf
-timeout_9: 10
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_test_topics.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_test_topics.launch
deleted file mode 100644
index 9bdc036369..0000000000
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_test_topics.launch
+++ /dev/null
@@ -1,9 +0,0 @@
-<!-- Check if the ROS topics using the fetch device are published. -->
-<!-- Usage: roslaunch jsk_fetch_startup fetch_test_topics.launch -->
-
-<launch>
-  <node name="test_device_topics"
-        pkg="jsk_tools" type="test_topic_published.py" output="screen">
-    <rosparam command="load" file="$(find jsk_fetch_startup)/config/test_device_topics.yaml" />
-  </node>
-</launch>

From f774ab2f963086f48356bcb04b9427a5b8667887 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Fri, 13 May 2022 02:57:16 +0900
Subject: [PATCH 3/5] [jsk_fetch_startup] Change prefix based on node name
 change

---
 jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml b/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml
index 5ccf32d376..eede96ea6e 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml
+++ b/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml
@@ -119,7 +119,7 @@ analyzers:
       joy:
         type: diagnostic_aggregator/GenericAnalyzer
         path: PS3 Controller
-        find_and_remove_prefix: "joy:"
+        find_and_remove_prefix: "jsk_joy_node:"
   system:
     type: diagnostic_aggregator/AnalyzerGroup
     path: System

From f42f90501b65db3a9bc0626f9c0561517bcbc87b Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Fri, 13 May 2022 02:58:08 +0900
Subject: [PATCH 4/5] [jsk_fetch_startup] Publish diagnostics for important
 topics and nodes

---
 .../jsk_fetch_startup/config/sanity_targets.yaml          | 7 +++++++
 .../jsk_fetch_startup/launch/fetch_bringup.launch         | 8 ++++++++
 2 files changed, 15 insertions(+)
 create mode 100644 jsk_fetch_robot/jsk_fetch_startup/config/sanity_targets.yaml

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/sanity_targets.yaml b/jsk_fetch_robot/jsk_fetch_startup/config/sanity_targets.yaml
new file mode 100644
index 0000000000..b320c32222
--- /dev/null
+++ b/jsk_fetch_robot/jsk_fetch_startup/config/sanity_targets.yaml
@@ -0,0 +1,7 @@
+topics:
+  - /audio
+  - /head_camera/depth/image_raw
+  - /head_camera/rgb/image_raw
+
+nodes:
+  - /head_camera/head_camera_nodelet_manager
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
index 08f3d294a2..bd0ad5f7bd 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -291,4 +291,12 @@
     </rosparam>
   </node>
 
+  <!-- Publish /diagnostics based on topics and nodes sanity check -->
+  <node name="sanity_diagnostics" pkg="jsk_tools" type="sanity_diagnostics.py">
+    <rosparam command="load" file="$(find jsk_fetch_startup)/config/sanity_targets.yaml" />
+    <rosparam subst_value="true">
+      duration: 60
+    </rosparam>
+  </node>
+
 </launch>

From 94b9250680ab95c35b44a9806d2ea5a971b23a58 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Fri, 13 May 2022 02:59:42 +0900
Subject: [PATCH 5/5] [jsk_fetch_startup] Add diagnostics aggregator targets

---
 .../config/fetch_analyzers.yaml               | 32 +++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml b/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml
index eede96ea6e..f4847a4cba 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml
+++ b/jsk_fetch_robot/jsk_fetch_startup/config/fetch_analyzers.yaml
@@ -144,3 +144,35 @@ analyzers:
         type: diagnostic_aggregator/GenericAnalyzer
         path: CPU Usage
         startswith: 'my_machine CPU Usage'
+  topics:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: Topics
+    analyzers:
+      audio:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: Audio
+        contains: '/audio'
+        timeout: 100
+        discard_stale: true
+      head_depth_image:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: HeadDepthImage
+        contains: '/head_camera/depth/image_raw'
+        timeout: 100
+        discard_stale: true
+      head_rgb_image:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: HeadRGBImage
+        contains: '/head_camera/rgb/image_raw'
+        timeout: 100
+        discard_stale: true
+  nodes:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: Nodes
+    analyzers:
+      head_camera_manager:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: HeadCameraManager
+        contains: '/head_camera/head_camera_nodelet_manager'
+        timeout: 100
+        discard_stale: true