[jsk_fetch_startup] Use actionlib in nav_speak.py

jsk_fetch_robot/jsk_fetch_startup/scripts/nav_speak.py

@@ -5,8 +5,10 @@
 import time
 
 from sound_play.libsoundplay import SoundClient
-from sound_play.msg import SoundRequestActionGoal
+from sound_play.msg import SoundRequestAction
+from sound_play.msg import SoundRequestGoal
 from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseActionResult
+import actionlib
 from actionlib_msgs.msg import GoalStatus
 
 
@@ -30,7 +32,8 @@ def __init__(self):
         self.lang = "japanese"  # speak japanese by default
         if rospy.has_param("/nav_speak/lang"):
             self.lang = rospy.get_param("/nav_speak/lang")
-        self.pub = rospy.Publisher('/robotsound_jp/goal', SoundRequestActionGoal, queue_size=1)
+        self.client = actionlib.SimpleActionClient('robotsound_jp', SoundRequestAction)
+        self.client.wait_for_server()
 
     def move_base_goal_callback(self, msg):
         self.sound.play(2)
@@ -39,34 +42,33 @@ def move_base_result_callback(self, msg):
         text = "{}: {}".format(goal_status(msg.status.status), msg.status.text)
         rospy.loginfo(text)
         if self.lang == "japanese":  # speak japanese
-            sound_goal = SoundRequestActionGoal()
-            sound_goal.goal_id.stamp = rospy.Time.now()
-            sound_goal.goal.sound_request.sound = -3
-            sound_goal.goal.sound_request.command = 1
-            sound_goal.goal.sound_request.volume = 1.0
-            sound_goal.goal.sound_request.arg2 = "jp"
+            sound_goal = SoundRequestGoal()
+            sound_goal.sound_request.sound = -3
+            sound_goal.sound_request.command = 1
+            sound_goal.sound_request.volume = 1.0
+            sound_goal.sound_request.arg2 = "jp"
         if msg.status.status == GoalStatus.SUCCEEDED:
             self.sound.play(1)
             time.sleep(1)
             if self.lang == "japanese":
-                sound_goal.goal.sound_request.arg = "到着しました"
-                self.pub.publish(sound_goal)
+                sound_goal.sound_request.arg = "到着しました"
+                self.client.send_goal(sound_goal)
             else:
                 self.sound.say(text)
         elif msg.status.status == GoalStatus.PREEMPTED:
             self.sound.play(2)
             time.sleep(1)
             if self.lang == "japanese":
-                sound_goal.goal.sound_request.arg = "別のゴールがセットされました"
-                self.pub.publish(sound_goal)
+                sound_goal.sound_request.arg = "別のゴールがセットされました"
+                self.client.send_goal(sound_goal)
             else:
                 self.sound.say(text)
         elif msg.status.status == GoalStatus.ABORTED:
             self.sound.play(3)
             time.sleep(1)
             if self.lang == "japanese":
-                sound_goal.goal.sound_request.arg = "中断しました"
-                self.pub.publish(sound_goal)
+                sound_goal.sound_request.arg = "中断しました"
+                self.client.send_goal(sound_goal)
             else:
                 self.sound.say(text)
         else: