[jsk_fetch_startup] Wait for other nodes to speak in nav_speak.py

jsk_fetch_robot/jsk_fetch_startup/scripts/nav_speak.py

@@ -9,7 +9,7 @@
 from sound_play.msg import SoundRequestGoal
 from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseActionResult
 import actionlib
-from actionlib_msgs.msg import GoalStatus
+from actionlib_msgs.msg import GoalStatus, GoalStatusArray
 
 
 def goal_status(status):
@@ -28,17 +28,22 @@ class NavSpeak:
     def __init__(self):
         self.move_base_goal_sub = rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.move_base_goal_callback, queue_size = 1)
         self.move_base_result_sub = rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.move_base_result_callback, queue_size = 1)
+        self.robotsound_jp_status_sub = rospy.Subscriber("/robotsound_jp/status", GoalStatusArray, self.robotsound_jp_status_callback, queue_size = 1)
         self.sound = SoundClient()
         self.lang = "japanese"  # speak japanese by default
         if rospy.has_param("/nav_speak/lang"):
             self.lang = rospy.get_param("/nav_speak/lang")
         self.client = actionlib.SimpleActionClient('robotsound_jp', SoundRequestAction)
         self.client.wait_for_server()
+        self.is_speaking = False
 
     def move_base_goal_callback(self, msg):
         self.sound.play(2)
 
     def move_base_result_callback(self, msg):
+        # Wait if other node is speaking
+        while self.is_speaking is True:
+            time.sleep(1)
         text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
         rospy.loginfo(text)
         if self.lang == "japanese":  # speak japanese
@@ -76,6 +81,12 @@ def move_base_result_callback(self, msg):
             time.sleep(1)
             self.sound.say(text)
 
+    def robotsound_jp_status_callback(self, msg):
+        if len(msg.status_list) == 0:
+            self.is_speaking = False
+        else:
+            self.is_speaking = True
+
 if __name__ == "__main__":
     global sound
     rospy.init_node("nav_speak")