From 282098153283399e112eff404df49b723dc563ec Mon Sep 17 00:00:00 2001
From: Furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 6 Dec 2017 17:11:04 +0900
Subject: [PATCH 1/2] Revert "pr2eus: add text-to-spech method to
 robot-interface (#318)"

This reverts commit ecb2a1e29d2d56ae16035064c91617f2b0afa786.
---
 pr2eus/robot-interface.l |  63 -----------------------
 pr2eus/speak.l           | 106 +++++++++++++++++++++++++--------------
 pr2eus/test/speak-test.l |  27 +++-------
 3 files changed, 74 insertions(+), 122 deletions(-)

diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
index 1f0844b7b..6906c7e00 100644
--- a/pr2eus/robot-interface.l
+++ b/pr2eus/robot-interface.l
@@ -4,7 +4,6 @@
 (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")
@@ -1040,7 +1039,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
@@ -1190,67 +1188,6 @@ 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)
-  (: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)
-;;
 (defclass robot-move-base-interface
   :super robot-interface
   :slots (move-base-action move-base-trajectory-action
diff --git a/pr2eus/speak.l b/pr2eus/speak.l
index edb383f59..a0aadc9b0 100644
--- a/pr2eus/speak.l
+++ b/pr2eus/speak.l
@@ -3,48 +3,76 @@
 
 (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 *speak-wait* nil)
+(defparameter *speak-action-clients* (make-hash-table))
+(defparameter *speak-timeout* 20)
 
-   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"
+(defun send-speak-msg (msg
+                       &key (topic-name "robotsound") (timeout *speak-timeout*)
+                            (wait *speak-wait*))
+  (cond
+    ((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))
+          (send goal :goal :sound_request msg)
+          (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)))
 
-  (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\")")
+(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)))
 
-  (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-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 wait timeout (topic-name "robotsound"))
-  "Speak english sentence using text-to-speech service.
-
-   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)))))
+(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))
 
 (provide :speak) ;; end of speak.l
diff --git a/pr2eus/test/speak-test.l b/pr2eus/test/speak-test.l
index fd77383a3..b51aacbb1 100644
--- a/pr2eus/test/speak-test.l
+++ b/pr2eus/test/speak-test.l
@@ -3,36 +3,23 @@
 
 (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")
 
 (init-unit-test)
 
-(defclass sample-robot-interface
-  :super robot-interface
-  :slots ())
-(defmethod sample-robot-interface
-  (:init
-   (&rest args)
-   (send-super* :init :robot sample-robot args)
-   self))
-
-(when (not (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" :timeout 10 :wait t))
+  (assert (speak-en "hello, world" :timeout 10 :google 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 "こんにちは" :timeout 10 :wait t))
+  (assert (speak-jp "こんにちは" :timeout 10 :google t)))
+
+(deftest test-speak-google ()
+  (assert (speak-google "bonjour" :timeout 10 :lang :fr)))
 
 (run-all-tests)
 (exit)

From cc85a9a791952962aa15bebdcc63a09d1fa45c11 Mon Sep 17 00:00:00 2001
From: Furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 6 Dec 2017 18:10:01 +0900
Subject: [PATCH 2/2] pr2eus: integrate speak function

---
 .travis                     |   2 +-
 pr2eus/robot-interface.l    |  15 +++++
 pr2eus/speak.l              | 122 +++++++++++++++++-------------------
 pr2eus/test/speak-test.l    |  41 +++++++++---
 pr2eus/test/speak-test.test |   2 +
 5 files changed, 108 insertions(+), 74 deletions(-)

diff --git a/.travis b/.travis
index 196da7d3b..9100c3e65 160000
--- a/.travis
+++ b/.travis
@@ -1 +1 @@
-Subproject commit 196da7d3be33444d6c0ca06bf2bce53725684d87
+Subproject commit 9100c3e65a4747cba6f98373f074341c342f6661
diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
index 6906c7e00..e4ffff6d3 100644
--- a/pr2eus/robot-interface.l
+++ b/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
diff --git a/pr2eus/speak.l b/pr2eus/speak.l
index a0aadc9b0..d0e69fb74 100644
--- a/pr2eus/speak.l
+++ b/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
diff --git a/pr2eus/test/speak-test.l b/pr2eus/test/speak-test.l
index b51aacbb1..18ac6b571 100644
--- a/pr2eus/test/speak-test.l
+++ b/pr2eus/test/speak-test.l
@@ -3,23 +3,44 @@
 
 (require :unittest "lib/llib/unittest.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
+  :super robot-interface
+  :slots ())
+(defmethod sample-robot-interface
+  (:init
+   (&rest args)
+   (send-super* :init :robot sample-robot args)
+   self))
+
+(unless (boundp '*ri*)
+  (setq *ri* (instance sample-robot-interface :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)
diff --git a/pr2eus/test/speak-test.test b/pr2eus/test/speak-test.test
index 432610ff6..efbd6fb03 100644
--- a/pr2eus/test/speak-test.test
+++ b/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">