pr2eus: partially revert speak function

pr2eus/robot-interface.l

@@ -4,11 +4,11 @@
 (ros::load-ros-manifest "roseus")
 (ros::load-ros-manifest "rosgraph_msgs")
 (ros::load-ros-manifest "control_msgs")
-(ros::load-ros-manifest "sound_play")
 ;;(ros::roseus-add-msgs "sensor_msgs") ;; roseus depends on sensor_msgs
 ;;(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
@@ -1040,7 +1040,6 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
    "get information about gripper"
    (error "This method is responsible to sub class~%"))
   ) ;; robot-interface
-
 ;; ros visualization methods
 (defmethod robot-interface
   (:joint-trajectory-to-angle-vector-list
@@ -1192,64 +1191,17 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
 ;;
 ;; for text-to-speech services
 (defmethod robot-interface
-  (:speak-timeout (&optional timeout)
-   (when timeout (send self :put :speak-timeout timeout))
-   (or (send self :get :speak-timeout) 10))
-  (:speak-action-client (&optional topic-name)
-   (unless (send self :get :speak-action-clients)
-     (return-from :speak-action-client nil))
-   (if topic-name
-       (gethash topic-name (send self :get :speak-action-clients))
-       (send (send self :get :speak-action-clients) :list-values)))
-  (:speak-raw (msg &key (topic-name "robotsound") wait)
-   (when (boundp 'sound_play::SoundRequestAction)
-     (unless (send self :speak-action-client)
-       (send self :put :speak-action-clients (make-hash-table :test #'equal)))
-     (let ((goal (instance sound_play::SoundRequestActionGoal :init))
-           (ac (or
-                (send self :speak-action-client topic-name)
-                (instance ros::simple-action-client :init
-                          topic-name
-                          sound_play::SoundRequestAction
-                          :groupname "speak"))))
-       (if (send ac :wait-for-server 1)
-           (progn
-             (send goal :goal :sound_request msg)
-             (setf (gethash topic-name
-                            (send self :get :speak-action-clients)) ac)
-             (if wait
-                 (send ac :send-goal-and-wait goal :timeout (send self :speak-timeout))
-                 (send ac :send-goal goal))
-             (return-from :speak-raw t))
-           (ros::ros-warn "action server /~A not found. continue without waiting for the end of speech.." topic-name))))
-   ;; use topic instead of actionlib
-   (unless (ros::get-topic-publisher topic-name)
-     (ros::advertise topic-name sound_play::SoundRequest 5)
-     (unix:sleep 1))
-   (ros::publish topic-name msg)
-   t)
+  (: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)
-    "Speak sentence using text-to-speech services.
-     Args:
-        text: sentence to speak
-        lang: language to speak, currently :en or :ja are supported.
-        wait: wait the end of speech if enabled"
-    (send self :speak-raw
-          (instance sound_play::SoundRequest :init
-                    :sound sound_play::SoundRequest::*say*
-                    :command sound_play::SoundRequest::*play_once*
-                    :arg text
-                    :arg2 (string-downcase lang))
-          :topic-name topic-name
-          :wait wait))
-  (:speak-en (text &rest args)
-    "Speak english sentence"
-    (send* self :speak text :lang "" args))
-  (:speak-jp (text &rest args &key (topic-name "robotsound_jp") &allow-other-keys)
-    "Speak japanese sentence"
-    (setq args (append args (list :topic-name topic-name)))
-    (send* self :speak text :lang :ja args))
-) ;; defmethod robot-interface (text-to-speech)
+   (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

pr2eus/speak.l

@@ -3,48 +3,72 @@
 
 (ros::load-ros-manifest "sound_play")
 
-(defun speak-jp (text &key wait timeout (topic-name "robotsound_jp"))
-  "Speak japanese sentence using text-to-speech service.
+(defparameter *sound-play-clients* (make-hash-table :test #'equal))
 
+(defun play-sound (sound &key arg2 (topic-name "robotsound") wait)
+  "Plays sound using sound_play server
    Args:
-    - text: sentence to speak
-    - topic-name: topic name space for sound_play server
-    - wait: wait the end of speech if enabled
-    - timeout: timeout for waiting, this is valid only if wait is enabled"
-
-  (unless (boundp '*ri*)
-    (ros::ros-error "Instantiate *ri* first to use text-to-speech.")
-    (return-from speak-jp nil))
-  (ros::ros-warn "The function `speak-jp` is deprecated, please use (send *ri* :speak-jp \"text\")")
-
-  (let ((timeout-bak (send *ri* :speak-timeout)))
-    (when timeout
-      (send *ri* :speak-timeout timeout))
-    (prog1
-        (send *ri* :speak-jp text :wait wait :topic-name topic-name)
-      (when timeout
-        (send *ri* :speak-timeout timeout-bak)))))
-
-(defun speak-en (text &key wait timeout (topic-name "robotsound"))
-  "Speak english sentence using text-to-speech service.
+     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))
+            (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
+              (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 (text &key (lang "") (topic-name "robotsound") wait)
+  "Speak sentence using text-to-speech services.
    Args:
-    - text: sentence to speak
-    - topic-name: topic name space for sound_play server
-    - wait: wait the end of speech if enabled
-    - timeout: timeout for waiting, this is valid only if wait is enabled"
-
-  (unless (boundp '*ri*)
-    (ros::ros-error "Instantiate *ri* first to use text-to-speech.")
-    (return-from speak-en nil))
-  (ros::ros-warn "The function `speak-en` is deprecated, please use (send *ri* :speak-en \"text\")")
-
-  (let ((timeout-bak (send *ri* :speak-timeout)))
-    (when timeout
-      (send *ri* :speak-timeout timeout))
-    (prog1
-        (send *ri* :speak-en text :wait wait :topic-name topic-name)
-      (when timeout
-        (send *ri* :speak-timeout timeout-bak)))))
+     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-en (text &key (topic-name "robotsound") wait)
+  "Speak english sentence"
+  (speak text :topic-name topic-name :wait wait))
+
+(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,12 +3,11 @@
 
 (require :unittest "lib/llib/unittest.l")
 
-(load "irteus/demo/sample-robot-model.l")
-(load "package://pr2eus/robot-interface.l")
-(load "package://pr2eus/speak.l")
-
 (ros::roseus "test_speak")
 
+(require :robot-interface "package://pr2eus/robot-interface.l")
+(load "irteus/demo/sample-robot-model.l")
+
 (init-unit-test)
 
 (defclass sample-robot-interface
@@ -20,19 +19,28 @@
    (send-super* :init :robot sample-robot args)
    self))
 
-(when (not (boundp '*ri*))
+(unless (boundp '*ri*)
   (setq *ri* (instance sample-robot-interface :init)))
 
-
 (deftest test-speak-en ()
-  (assert (speak-en "hello, world" :timeout 10))
-  (assert (send *ri* :speak-en "hello, world"))
-  (assert (send *ri* :speak-en "good bye!" :wait t)))
+  (assert (speak-en "hello, world"))
+  (assert (speak-en "hello, world" :wait t))
+)
 
 (deftest test-speak-jp ()
-  (assert (speak-jp "こんにちは" :timeout 10))
-  (assert (send *ri* :speak-jp "こんにちは"))
-  (assert (send *ri* :speak-jp "またね" :wait t)))
+  (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)

pr2eus/test/speak-test.test

@@ -1,4 +1,6 @@
 <launch>
+  <env name="DISPLAY" value=":0.0" />
+
   <test test-name="speak_test_node" pkg="roseus" type="roseus"
         args="$(find pr2eus)/test/speak-test.l" time-limit="1800" />
   <node pkg="sound_play" type="soundplay_node.py" name="sound_play">