pr2eus: integrate speak function

pr2eus/robot-interface.l

@@ -8,6 +8,7 @@
 ;;(ros::roseus-add-msgs "visualization_msgs") ;; roseus depends on visualization_msgs
 (ros::roseus-add-msgs "move_base_msgs")
 (ros::roseus-add-msgs "nav_msgs")
+(require :speak "package://pr2eus/speak.l")
 
 ;; add ros-joint-angle method using meter/radian
 (defmethod rotational-joint
@@ -1188,6 +1189,20 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
    )
   )
 ;;
+;; for text-to-speech services
+(defmethod robot-interface
+  (:play-sound (sound &key arg2 (topic-name "robotsound") wait)
+   (funcall #'play-sound sound :arg2 arg2 :topic-name topic-name :wait wait))
+  (:speak (text &key (lang "") (topic-name "robotsound") wait)
+   (send self :play-sound text
+         :topic-name topic-name
+         :wait wait
+         :arg2 (if (keywordp lang) (string-downcase lang) lang)))
+  (:speak-en (text &key (topic-name "robotsound") wait)
+   (send self :speak text :topic-name topic-name :wait wait))
+  (:speak-jp (text &key (topic-name "robotsound_jp") wait)
+   (send self :speak text :lang :ja :topic-name topic-name :wait wait)))
+;;
 (defclass robot-move-base-interface
   :super robot-interface
   :slots (move-base-action move-base-trajectory-action

pr2eus/speak.l

@@ -3,76 +3,72 @@
 
 (ros::load-ros-manifest "sound_play")
 
-(defparameter *speak-wait* nil)
-(defparameter *speak-action-clients* (make-hash-table))
-(defparameter *speak-timeout* 20)
+(defparameter *sound-play-clients* (make-hash-table :test #'equal))
 
-(defun send-speak-msg (msg
-                       &key (topic-name "robotsound") (timeout *speak-timeout*)
-                            (wait *speak-wait*))
-  (cond
-    ((boundp 'sound_play::soundrequestaction)
+(defun play-sound (sound &key arg2 (topic-name "robotsound") wait)
+  "Plays sound using sound_play server
+   Args:
+     sound: if sound is pathname, plays sound file located at given path
+            if it is number, server plays builtin sound
+            otherwise server plays sound as speech sentence
+     topic-name: namespace of sound_play server
+     wait: wait until sound is played"
+  (let ((msg (instance sound_play::SoundRequest :init
+                       :command sound_play::SoundRequest::*play_once*)))
+    (cond
+      ((numberp sound)
+       (send msg :sound sound))
+      ((pathnamep sound)
+       (send msg :sound sound_play::SoundRequest::*play_file*)
+       (send msg :arg sound))
+      (t
+       (send msg :sound sound_play::SoundRequest::*say*)
+       (send msg :arg (string sound))))
+    (if arg2 (send msg :arg2 arg2))
+
+    (when (boundp 'sound_play::SoundRequestAction)
       (let ((goal (instance sound_play::SoundRequestActionGoal :init))
-            (action-client-key (intern (string-upcase topic-name) *keyword-package*)))
-        (unless (gethash action-client-key *speak-action-clients*)
-          (setf (gethash action-client-key *speak-action-clients*)
-                (instance ros::simple-action-client :init
-                          topic-name sound_play::SoundRequestAction :groupname "speak")))
-        (let ((ac (gethash action-client-key *speak-action-clients*)))
-          (unless (send ac :wait-for-server timeout)
-            (ros::ros-error "action server /~A is not found. sound_play node is not alive?" topic-name)
-            (return-from send-speak-msg nil))
+            (ac (or (gethash topic-name *sound-play-clients*)
+                    (instance ros::simple-action-client :init
+                              topic-name sound_play::SoundRequestAction
+                              :groupname "sound_play"))))
+        (when (send ac :wait-for-server 1)
+          (when (eq (send ac :get-state) actionlib_msgs::GoalStatus::*active*)
+            (send ac :cancel-goal)
+            (send ac :wait-for-result :timeout 10))
           (send goal :goal :sound_request msg)
+          (setf (gethash topic-name *sound-play-clients*) ac)
           (send ac :send-goal goal)
           (if wait
-            (send ac :wait-for-result :timeout timeout) t))))
-    (t ;; action client is not used for backward compatibility
-      (unless (ros::get-topic-publisher topic-name)
-        (ros::advertise topic-name sound_play::SoundRequest 5)
-        (unix:sleep 1))
-      (ros::publish topic-name msg)
-      t)))
+              (return-from play-sound (send ac :wait-for-result :timeout 10))
+              (return-from play-sound t)))))
+    ;; use publisher
+    (ros::ros-warn "action server /~A not found." topic-name)
+    (unless (ros::get-topic-publisher topic-name)
+      (ros::advertise topic-name sound_play::SoundRequest 5)
+      (unix:sleep 1))
+    (ros::publish topic-name msg)
+    t))
 
-(defun speak-google (str &key (lang :ja) (wait *speak-wait*) (topic-name "robotsound") (timeout *speak-timeout*))
-  (let* ((qstr (escaped-url-string-from-namestring
-                (concatenate string
-                             "http://translate.google.com/translate_tts?tl="
-                             (string-downcase (string lang))
-                             "&client=t&ie=UTF-8&q=" str)))
-         (msg (instance sound_play::SoundRequest :init
-                        :sound sound_play::SoundRequest::*play_file*
-                        :command sound_play::SoundRequest::*play_once*
-                        :arg qstr)))
-    (send-speak-msg msg
-                    :topic-name topic-name
-                    :wait wait
-                    :timeout timeout)))
+(defun speak (text &key (lang "") (topic-name "robotsound") wait)
+  "Speak sentence using text-to-speech services.
+   Args:
+     text: sentence to speak
+     lang: language to speak, currently :en or :ja are supported.
+     topic-name: namespace of sound_play node
+     wait: wait the end of speech if enabled"
+  (play-sound text
+              :topic-name topic-name
+              :wait wait
+              :arg2 (if (keywordp lang)
+                        (string-downcase lang) lang)))
 
-(defun speak-jp (str &key google (wait *speak-wait*) (topic-name "robotsound_jp") (timeout *speak-timeout*))
-  (when google
-      (return-from speak-jp
-        (speak-google str :lang :ja :wait wait :timeout timeout)))
-  (send-speak-msg
-   (instance sound_play::SoundRequest :init
-             :sound sound_play::SoundRequest::*say*
-             :command sound_play::SoundRequest::*play_once*
-             :arg str
-             :arg2 "aq_rm.phont")
-   :topic-name topic-name
-   :wait wait
-   :timeout timeout))
+(defun speak-en (text &key (topic-name "robotsound") wait)
+  "Speak english sentence"
+  (speak text :topic-name topic-name :wait wait))
 
-(defun speak-en (str &key google (wait *speak-wait*) (topic-name "robotsound") (timeout *speak-timeout*))
-  (when google
-    (return-from speak-en
-            (speak-google str :lang :en :wait wait :timeout timeout)))
-  (send-speak-msg
-   (instance sound_play::SoundRequest :init
-             :sound sound_play::SoundRequest::*say*
-             :command sound_play::SoundRequest::*play_once*
-             :arg str)
-   :topic-name topic-name
-   :wait wait
-   :timeout timeout))
+(defun speak-jp (text &key (topic-name "robotsound_jp") wait)
+  "Speak japanese sentence"
+  (speak text :lang :ja :topic-name topic-name :wait wait))
 
 (provide :speak) ;; end of speak.l

pr2eus/test/speak-test.l

@@ -3,23 +3,32 @@
 
 (require :unittest "lib/llib/unittest.l")
 
-(load "package://pr2eus/speak.l")
+(load "package://pr2eus/pr2-interface.l")
 (ros::roseus "test_speak")
 
 (init-unit-test)
 
+(unless (boundp '*ri*)
+  (pr2-init))
+
 (deftest test-speak-en ()
-  (assert (speak-en "hello, world" :timeout 10))
-;;  (assert (speak-en "hello, world" :timeout 10 :wait t))
-  (assert (speak-en "hello, world" :timeout 10 :google t)))
+  (assert (speak-en "hello, world"))
+;;  (assert (speak-en "hello, world" :wait t))
+)
 
 (deftest test-speak-jp ()
-  (assert (speak-jp "こんにちは" :timeout 10))
-;;  (assert (speak-jp "こんにちは" :timeout 10 :wait t))
-  (assert (speak-jp "こんにちは" :timeout 10 :google t)))
-
-(deftest test-speak-google ()
-  (assert (speak-google "bonjour" :timeout 10 :lang :fr)))
+  (assert (speak-jp "こんにちは"))
+;;  (assert (speak-jp "こんにちは" :wait t))
+)
+
+(deftest test-ri-speak-en ()
+  (assert (send *ri* :speak-en "hello, world"))
+;;  (assert (send *ri* :speak-en "hello, world" :wait t)))
+
+(deftest test-ri-speak-jp ()
+  (assert (send *ri* :speak-jp "やあ"))
+;;  (assert (send *ri* :speak-jp "やあ" :wait t))
+)
 
 (run-all-tests)
 (exit)