[jsk_pr2_startup] Add cable connector check warning

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/battery_warning.py

@@ -23,6 +23,8 @@ def __init__(self):
         # speak
         self.speak_client = actionlib.SimpleActionClient(
             "/robotsound_jp", SoundRequestAction)
+        self.sound_client = actionlib.SimpleActionClient(
+            "/robotsound", SoundRequestAction)
         ok = self.speak_client.wait_for_server(rospy.Duration(10))
         if not ok:
             rospy.logfatal("Action /robotsound_jp is not advertised")
@@ -31,6 +33,7 @@ def __init__(self):
         # param
         self.monitor_rate = rospy.get_param("~monitor_rate", 4)
         self.warning_temp = rospy.get_param("~warning_temperature", 42.0)
+        self.cable_warning_temp = rospy.get_param("~cable_warning_temperature", 44.0)
         self.min_capacity = rospy.get_param("~min_capacity", 800)
         self.warning_voltage = rospy.get_param("~warning_voltage", 14.0)
         self.critical_voltage = rospy.get_param("~critical_voltage", 13.7)
@@ -41,6 +44,7 @@ def __init__(self):
         self.prev_plugged_in = None
         self.latest_status = None
         self.speech_history = defaultdict(lambda: rospy.Time(0))
+        self.sound_history = defaultdict(lambda: rospy.Time(0))
 
         self.diag_sub = rospy.Subscriber(
             "/diagnostics_agg", DiagnosticArray, self.diag_cb, queue_size=1)
@@ -66,6 +70,20 @@ def speak(self, sentence):
         self.speak_client.send_goal(SoundRequestGoal(sound_request=req))
         self.speak_client.wait_for_result(timeout=rospy.Duration(10))
 
+    def play_sound(self, sound):
+        key = sound
+        if self.sound_history[key] + rospy.Duration(self.warn_repeat_rate) > rospy.Time.now():
+            return
+        self.sound_history[key] = rospy.Time.now()
+        sound_req = SoundRequest()
+        sound_req.sound = sound
+        sound_req.command = SoundRequest.PLAY_ONCE  # 1: PLAY_ONCE
+        sound_req.volume = 1.0  # Volume（0.0〜1.0）
+        sound_req.arg = ''
+        sound_req.arg2 = ''
+        self.sound_client.send_goal(SoundRequestGoal(sound_request=sound_req))
+        self.sound_client.wait_for_result(timeout=rospy.Duration(10))
+
     def log_cb(self, event):
         try:
             if osp.exists(self.log_path):
@@ -94,6 +112,9 @@ def stat_cb(self, event):
             rospy.logdebug("temperature: %s" % max_temp)
             if 60 > max_temp > self.warning_temp:
                 self.speak("バッテリ温度%.1f度。暑いです。部屋の温度を下げてください。" % max_temp)
+                if max_temp > self.cable_warning_temp:
+                    self.play_sound(2) # kan kan
+                    self.speak("危険な温度です。電源ケーブルのコネクタを確認して下さい。")
         except KeyError:
             pass
         except ValueError: