[wait for upstream] control tweet speak volume and speak enable by dynamic reconfigure

jsk_robot_common/jsk_robot_startup/lifelog/tweet_client.l

@@ -6,7 +6,8 @@
 
 (load "package://pr2eus/speak.l")
 
-(defun tweet-string (twit-str &key (warning-time) (with-image) (image-wait 30) (speak t))
+(defun tweet-string (twit-str &key (warning-time) (with-image) (image-wait 30) (speak t)
+                              (volume 1.0))
   (let (prev-image-topic img)
   (when warning-time
     (unless (numberp warning-time)
@@ -25,15 +26,17 @@
                           (8 "はち")
                           (9 "きゅう")
                           (10 "じゅう")
-                          (t "じゅういじょう")))))
+                          (t "じゅういじょう")))
+                :volume volume))
     (unix::sleep warning-time))
 
   (cond
     (with-image
       (unix::system (format nil "rm -f /tmp/tweet_image.jpg"))
       ;; camera shot sound
-      (play-sound (pathname (ros::resolve-ros-path "package://jsk_pr2_startup/jsk_pr2_lifelog/camera.wav"))
-                  :topic-name "robotsound_jp" :wait t)
+      (when speak
+        (play-sound (pathname (ros::resolve-ros-path "package://jsk_pr2_startup/jsk_pr2_lifelog/camera.wav"))
+                    :topic-name "robotsound_jp" :wait t :volume volume))
       ;; specify camera
       (when (stringp with-image)
         (ros::wait-for-service "/tweet_image_mux/list")
@@ -61,5 +64,5 @@
       (ros::service-call "/tweet_image_mux/select" (instance topic_tools::muxselectrequest :init :topic prev-image-topic)))
     (t
       (ros::publish "/tweet" (instance std_msgs::String :init :data twit-str))))
-  (when speak (speak-jp "ついーとしました" :wait t))))
+  (when speak (speak-jp "ついーとしました" :wait t :volume volume))))
 

jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l

@@ -3,13 +3,42 @@
 (ros::roseus "twitter_client_tablet")
 (ros::roseus-add-msgs "roseus")
 
+(load "package://roseus/euslisp/dynamic-reconfigure-server.l")
 (load "package://jsk_robot_startup/lifelog/tweet_client.l")
 
+(setq *volume* (ros::get-param "~volume" 1.0))
+(setq *speak-enable* (ros::get-param "~speak_enable" t))
+(setq *enable* (ros::get-param "~enable" t))
+(setq *reconfigure-server*
+      (def-dynamic-reconfigure-server
+        ;;; ((name type level description (default) (min) (max) (edit_method)) ... )
+        (("volume"  double_t  0  "tweet speak volume" 1.0  0.0 1.0)
+         ("speak_enable"  bool_t  0  "tweet speak enable" t)
+         ("enable"  bool_t  0  "tweet enable" t))
+        ;; use lamda-closure to avoid memory error
+        '(lambda-closure nil 0 0 (cfg level)
+            (let ((prev-volume *volume*)
+                  (prev-speak-enable *speak-enable*)
+                  (prev-enable *enable*))
+              (setq *volume* (cdr (assoc "volume" cfg :test #'equal)))
+              (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal)))
+              (setq *enable* (cdr (assoc "enable" cfg :test #'equal)))
+              (if (null (equal *volume* prev-volume))
+                (ros::ros-warn "Volume changed to: ~A" *volume*))
+              (if (null (equal *enable* prev-enable))
+                (ros::ros-warn "Enable changed to: ~A" *enable*))
+              (if (null (equal *speak-enable* prev-speak-enable))
+                (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*)))
+            cfg)))
+
 (defun twit-cb (msg)
-  (let ((twit-str (send msg :data)))
-    (tweet-string twit-str
-                  :warning-time nil
-                  :with-image "/tablet/marked/image_rect_color")))
+  (if *enable*
+    (let ((twit-str (send msg :data)))
+      (tweet-string twit-str
+                    :warning-time nil
+                    :with-image "/tablet/marked/image_rect_color"
+                    :speak *speak-enable*
+                    :volume *volume*))))
 
 (ros::advertise "/tweet" std_msgs::String 1)
 (ros::subscribe "/pr2twit_from_tablet" roseus::StringStamped #'twit-cb)

jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l

@@ -2,6 +2,7 @@
 
 (ros::roseus "twitter_client_uptime")
 
+(load "package://roseus/euslisp/dynamic-reconfigure-server.l")
 (load "package://jsk_robot_startup/lifelog/tweet_client.l")
 
 (setq *src-lines* nil)
@@ -37,52 +38,81 @@
  (t
   (setq *waking-target-second* *waking-tweet-second*)))
 
+(setq *volume* (ros::get-param "~volume" 1.0))
+(setq *speak-enable* (ros::get-param "~speak_enable" t))
+(setq *enable* (ros::get-param "~enable" t))
+(setq *reconfigure-server*
+      (def-dynamic-reconfigure-server
+        ;;; ((name type level description (default) (min) (max) (edit_method)) ... )
+        (("volume"  double_t  0  "tweet speak volume" 1.0  0.0 1.0)
+         ("speak_enable"  bool_t  0  "tweet speak enable" t)
+         ("enable"  bool_t  0  "tweet enable" t))
+        ;; use lamda-closure to avoid memory error
+        '(lambda-closure nil 0 0 (cfg level)
+            (let ((prev-volume *volume*)
+                  (prev-speak-enable *speak-enable*)
+                  (prev-enable *enable*))
+              (setq *volume* (cdr (assoc "volume" cfg :test #'equal)))
+              (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal)))
+              (setq *enable* (cdr (assoc "enable" cfg :test #'equal)))
+              (if (null (equal *volume* prev-volume))
+                (ros::ros-warn "Volume changed to: ~A" *volume*))
+              (if (null (equal *enable* prev-enable))
+                (ros::ros-warn "Enable changed to: ~A" *enable*))
+              (if (null (equal *speak-enable* prev-speak-enable))
+                (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*)))
+            cfg)))
+
 (ros::advertise "/tweet" std_msgs::String 1)
 (ros::rate 0.1)
 (do-until-key
-  (setq *user-name* (ros::get-param "/active_user/launch_user_name")
-        *elapsed-time* (ros::get-param "/active_user/elapsed_time"))
-  ;; tweet depend on up time
-  (let ((st (ros::get-param "/active_user/start_time")))
-    (when st
-      (let ((waking-time (- (send (ros::time-now) :to-sec) st)))
-        (ros::ros-debug "~A waking ~A sec (~A)" *robot-name* waking-time *waking-target-second*)
-        (when (> waking-time *waking-target-second*)
-          (incf *waking-target-second* *waking-tweet-second*)
-          ;; tweet source of robot-interface
-          (unless *src-lines*
-            (let* ((dirname (ros::rospack-find "pr2eus"))
-                   (fname   (format nil "~A/robot-interface.l" dirname))
-                   str)
-              (with-open-file (f fname)
-                (while (setq str (read-line f nil nil))
-                (push str *src-lines*)))
-              (setq *src-lines* (nreverse *src-lines*))
-              ))
+  (if *enable*
+    (progn
+      (setq *user-name* (ros::get-param "/active_user/launch_user_name")
+            *elapsed-time* (ros::get-param "/active_user/elapsed_time"))
+      ;; tweet depend on up time
+      (let ((st (ros::get-param "/active_user/start_time")))
+        (when st
+          (let ((waking-time (- (send (ros::time-now) :to-sec) st)))
+            (ros::ros-debug "~A waking ~A sec (~A)" *robot-name* waking-time *waking-target-second*)
+            (when (> waking-time *waking-target-second*)
+              (incf *waking-target-second* *waking-tweet-second*)
+              ;; tweet source of robot-interface
+              (unless *src-lines*
+                (let* ((dirname (ros::rospack-find "pr2eus"))
+                       (fname   (format nil "~A/robot-interface.l" dirname))
+                       str)
+                  (with-open-file (f fname)
+                    (while (setq str (read-line f nil nil))
+                    (push str *src-lines*)))
+                  (setq *src-lines* (nreverse *src-lines*))
+                  ))
 
-          (let* ((len (length *src-lines*))
-                 (start-n (floor (random (float len) *random-state*)))
-                 (spos 0) (str-len 0) lines)
-            (push (format nil "I am running ~A min." (round (/ waking-time 60.0)))
-                  lines)
-            (incf str-len (length (car lines)))
-            (while (< (+ start-n spos) len)
-              (let ((str (elt *src-lines* (+ start-n spos))))
-                (incf str-len (length str))
-                (if (> str-len 140) (return))
-                (push str lines))
-              (incf spos))
-            (let* ((ln (apply #'+ (length lines)
-                              (mapcar #'(lambda (x) (length x)) lines)))
-                   (dt (make-string (1- ln)))
-                   (pos 0))
-              (dolist (s (nreverse lines))
-                (replace dt s :start1 pos)
-                (incf pos (length s))
-                (if (< pos (- ln 2)) (setf (elt dt pos) 10))
-                (incf pos))
-              (tweet-string dt :warning-time 1 :with-image t)
-              ))
-          ))))
+              (let* ((len (length *src-lines*))
+                     (start-n (floor (random (float len) *random-state*)))
+                     (spos 0) (str-len 0) lines)
+                (push (format nil "I am running ~A min." (round (/ waking-time 60.0)))
+                      lines)
+                (incf str-len (length (car lines)))
+                (while (< (+ start-n spos) len)
+                  (let ((str (elt *src-lines* (+ start-n spos))))
+                    (incf str-len (length str))
+                    (if (> str-len 140) (return))
+                    (push str lines))
+                  (incf spos))
+                (let* ((ln (apply #'+ (length lines)
+                                  (mapcar #'(lambda (x) (length x)) lines)))
+                       (dt (make-string (1- ln)))
+                       (pos 0))
+                  (dolist (s (nreverse lines))
+                    (replace dt s :start1 pos)
+                    (incf pos (length s))
+                    (if (< pos (- ln 2)) (setf (elt dt pos) 10))
+                    (incf pos))
+                  (tweet-string dt :warning-time 1 :with-image t
+                                :speak *speak-enable*
+                                :volume *volume*)
+                  ))))))))
+  (ros::spin-once)
   (ros::sleep)
   )

jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l

@@ -2,41 +2,67 @@
 
 (ros::roseus "twitter_client_warning")
 
+(load "package://roseus/euslisp/dynamic-reconfigure-server.l")
 (load "package://jsk_robot_startup/lifelog/tweet_client.l")
 (ros::load-ros-manifest "diagnostic_msgs")
 
+(setq *volume* (ros::get-param "~volume" 1.0))
+(setq *speak-enable* (ros::get-param "~speak_enable" t))
+(setq *enable* (ros::get-param "~enable" t))
+(setq *reconfigure-server*
+      (def-dynamic-reconfigure-server
+        ;;; ((name type level description (default) (min) (max) (edit_method)) ... )
+        (("volume"  double_t  0  "tweet speak volume" 1.0  0.0 1.0)
+         ("speak_enable"  bool_t  0  "tweet speak enable" t)
+         ("enable"  bool_t  0  "tweet enable" t))
+        ;; use lamda-closure to avoid memory error
+        '(lambda-closure nil 0 0 (cfg level)
+            (let ((prev-volume *volume*)
+                  (prev-speak-enable *speak-enable*)
+                  (prev-enable *enable*))
+              (setq *volume* (cdr (assoc "volume" cfg :test #'equal)))
+              (setq *speak-enable* (cdr (assoc "speak_enable" cfg :test #'equal)))
+              (setq *enable* (cdr (assoc "enable" cfg :test #'equal)))
+              (if (null (equal *volume* prev-volume))
+                (ros::ros-warn "Volume changed to: ~A" *volume*))
+              (if (null (equal *enable* prev-enable))
+                (ros::ros-warn "Enable changed to: ~A" *enable*))
+              (if (null (equal *speak-enable* prev-speak-enable))
+                (ros::ros-warn "Speak enable changed to: ~A" *speak-enable*)))
+            cfg)))
+
 (defun diagnostics-cb (msg)
-  (let ((diagnostics (make-hash-table :test #'equal))
-	(tm (ros::time-now))
-	status id)
-    (ros::ros-debug (format nil "~0,3f diagnostics_msgs~%" (send tm :to-sec)))
-    (dolist (status (send msg :status))
-      ;; diagnostic_msgs::DiagnosticStatus::*WARN*
-      (when (>= (send status :level) diagnostic_msgs::DiagnosticStatus::*WARN*) ;; diagnostic_msgs::DiagnosticStatus::*ERROR*)
-	(cond ((substringp "/Motors" (send status :name))
-	       t) ;; skip motors
-	      ((substringp "/Other/Accelerometer" (send status :name)) t)
-	      ((substringp "/Other/Pressure" (send status :name)) t)
-	      ((and (string= "/Computers/Network/Wifi Status (ddwrt)" (send status :name))
-		    (string= "Updates Stale" (send status :message))) t)
-	      ((and (string= "/Computers/Network" (send status :name))
-		    (string= "Error" (send status :message))) t)
-              ((substringp "/Peripherals/PS3 Controller" (send status :name)) t) ;; fetch joystick warning
-	      ((position #\/ (send status :name) :count 2) ;; check depth of name
-	       (setq key (subseq (send status :name) 0 (position #\/ (send status :name) :count 2)))
-	       (when (> (length (send status :name)) (length (gethash key diagnostics)))
-		 (setf (gethash key diagnostics) (cons (send status :name) (send status :message)))
-		 ) ;; when
-	       ))
-	)) ;; when / dolist
-    (maphash #'(lambda (k v) (ros::ros-debug (format nil "Warnings ~A ~A~%" (length status) v)) (push v status)) diagnostics)
-    (when status
-      (setq id (random (length status)))
-      (when (= (mod (round (send tm :sec)) 1000) 0)
-        (tweet-string (format nil "Warning!! ~A is ~A at ~0,3f" (car (elt status id)) (cdr (elt status id)) (send tm :to-sec))
-                      :warning-time 1 :with-image t)
-	)) ;; when
-    )) ;; let
+  (if *enable*
+    (let ((diagnostics (make-hash-table :test #'equal))
+          (tm (ros::time-now))
+          status id)
+      (ros::ros-debug (format nil "~0,3f diagnostics_msgs~%" (send tm :to-sec)))
+      (dolist (status (send msg :status))
+        ;; diagnostic_msgs::DiagnosticStatus::*WARN*
+        (when (>= (send status :level) diagnostic_msgs::DiagnosticStatus::*WARN*) ;; diagnostic_msgs::DiagnosticStatus::*ERROR*)
+          (cond ((substringp "/Motors" (send status :name))
+                 t) ;; skip motors
+                ((substringp "/Other/Accelerometer" (send status :name)) t)
+                ((substringp "/Other/Pressure" (send status :name)) t)
+                ((and (string= "/Computers/Network/Wifi Status (ddwrt)" (send status :name))
+                      (string= "Updates Stale" (send status :message))) t)
+                ((and (string= "/Computers/Network" (send status :name))
+                      (string= "Error" (send status :message))) t)
+                ((substringp "/Peripherals/PS3 Controller" (send status :name)) t) ;; fetch joystick warning
+                ((position #\/ (send status :name) :count 2) ;; check depth of name
+                 (setq key (subseq (send status :name) 0 (position #\/ (send status :name) :count 2)))
+                 (when (> (length (send status :name)) (length (gethash key diagnostics)))
+                   (setf (gethash key diagnostics) (cons (send status :name) (send status :message)))
+                   ) ;; when
+                 ))
+          )) ;; when / dolist
+      (maphash #'(lambda (k v) (ros::ros-debug (format nil "Warnings ~A ~A~%" (length status) v)) (push v status)) diagnostics)
+      (when status
+        (setq id (random (length status)))
+        (when (= (mod (round (send tm :sec)) 1000) 0)
+          (tweet-string (format nil "Warning!! ~A is ~A at ~0,3f" (car (elt status id)) (cdr (elt status id)) (send tm :to-sec))
+                        :warning-time 1 :with-image t :volume *volume* :speak *speak-enable*)))
+      )))
 
 (ros::advertise "/tweet" std_msgs::String 1)
 (ros::subscribe "/diagnostics_agg" diagnostic_msgs::DiagnosticArray #'diagnostics-cb)