From daac8c34bbe6f3a3e561789b533200071c9e0d6c Mon Sep 17 00:00:00 2001
From: iory <ab.ioryz@gmail.com>
Date: Thu, 19 May 2022 22:31:48 +0900
Subject: [PATCH] [jsk_pr2_startup] Add audible warning and sanity diagnostics

---
 .../jsk_pr2_warning/sanity_targets.yaml       |  4 ++++
 jsk_pr2_robot/jsk_pr2_startup/pr2.launch      | 21 +++++++++++++++++++
 2 files changed, 25 insertions(+)
 create mode 100644 jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml

diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml
new file mode 100644
index 0000000000..effacff7b8
--- /dev/null
+++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/sanity_targets.yaml
@@ -0,0 +1,4 @@
+topics:
+  - /kinect_head/rgb/image_raw
+  - /kinect_head/depth_registered/image_raw
+
diff --git a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
index 53dc7ca6b7..d7ea7e0792 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
+++ b/jsk_pr2_robot/jsk_pr2_startup/pr2.launch
@@ -197,4 +197,25 @@
     <remap from="~input" to="/kinect_head/rgb/image_raw" />
   </node>
 
+  <node name="audible_warning"
+        pkg="jsk_tools" type="audible_warning.py"
+        clear_params="true"
+        output="screen" >
+    <rosparam command="load" file="$(find jsk_pr2_startup)/jsk_pr2_warning/warning_blacklist.yaml" />
+    <rosparam>
+      run_stop_topic: /power_board/state
+      run_stop_condition: "m.run_stop is False"
+      seconds_to_start_speaking: 60
+      speak_interval: 600
+    </rosparam>
+  </node>
+
+  <node name="sanity_diagnostics"
+        pkg="jsk_tools" type="sanity_diagnostics.py" >
+    <rosparam command="load" file="$(find jsk_pr2_startup)/jsk_pr2_warning/sanity_targets.yaml" />
+    <rosparam subst_value="true">
+      duration: 60
+    </rosparam>
+  </node>
+
 </launch>