Merge branch 'master' into use-follow-base-traj

jsk-ros-pkg · Jul 11, 2016 · 811a62b · 811a62b
2 parents bf12051 + d81845b
commit 811a62b
Showing 1 changed file with 21 additions and 14 deletions.
diff --git a/pr2eus/speak.l b/pr2eus/speak.l
@@ -4,21 +4,28 @@
 (ros::load-ros-manifest "sound_play")
 
 (defparameter *speak-wait* nil)
+(defparameter *speak-action-clients* (make-hash-table))
 
-(defun send-speak-msg (msg &key (topic-name "robotsound") (timeout 0) (wait *speak-wait*))
-  (if (boundp 'sound_play::soundrequestaction)
-      (let ((ac (instance ros::simple-action-client :init
-                          topic-name sound_play::SoundRequestAction :groupname "speak"))
-            (goal (instance sound_play::SoundRequestActionGoal :init)))
-        (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))
-    ;; for backward compatibility
-    (progn
+(defun send-speak-msg (msg
+                       &key (topic-name "robotsound") (timeout 0)
+                            (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))