From 1aed0a75283f39fe13585cb1856234f9132a8c41 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 18 Nov 2022 16:42:46 +0900 Subject: [PATCH] add enable and speak-enable dynamic reconfigure --- .../lifelog/tweet_client_tablet.l | 33 ++++-- .../lifelog/tweet_client_uptime.l | 110 ++++++++++-------- .../lifelog/tweet_client_warning.l | 82 +++++++------ .../lifelog/tweet_client_worktime.l | 104 ++++++++++------- 4 files changed, 196 insertions(+), 133 deletions(-) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l index d460a6bb32..9d75435b97 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l @@ -7,22 +7,39 @@ (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)) + (("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) - (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) - (ros::ros-warn "Volume changed to: ~A" *volume*) + (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" - :volume *volume*))) + (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::subscribe "/pr2twit_from_tablet" roseus::StringStamped #'twit-cb) (ros::spin) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l index 9db14a8d01..35c1d94bc9 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l @@ -14,63 +14,79 @@ ) (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)) + (("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) - (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) - (ros::ros-warn "Volume changed to: ~A" *volume*) + (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::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 - :volume *volume*) - )) - )))) + (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) ) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l index 631e1366cf..f85e8726e7 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l @@ -7,48 +7,62 @@ (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)) + (("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) - (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) - (ros::ros-warn "Volume changed to: ~A" *volume*) + (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 :volume *volume*) - )) ;; 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*))) + ))) (ros::subscribe "/diagnostics_agg" diagnostic_msgs::DiagnosticArray #'diagnostics-cb) (ros::rate (/ 1.0 3.0)) diff --git a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l index 10d1073740..e889f4fa8f 100755 --- a/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l +++ b/jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l @@ -11,60 +11,76 @@ ) (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)) + (("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) - (setq *volume* (cdr (assoc "volume" cfg :test #'equal))) - (ros::ros-warn "Volume changed to: ~A" *volume*) + (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::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")) - (ros::ros-debug "user -> ~A, time -> ~A (~A) " - *user-name* *elapsed-time* *target-second*) - ;; tweet depend on working time - (when (> *elapsed-time* *target-second*) - (incf *target-second* *tweet-second* ) - (ros::ros-info "tweet ~A ~A" *user-name* *elapsed-time*) - (let ((mainstr (format nil "~A has used ~A for ~d minutes" - ;; why delete *user-name* after space ? - ;;(subseq *user-name* 0 - ;;(or (position #\space *user-name*) - ;;(length *user-name*))) - *user-name* - *robot-name* - (round (/ *elapsed-time* 60)))) - presubstr postsubstr) - (cond - ((< *elapsed-time* 600) ;; 5 min - (setq presubstr "Congratulations! " - postsubstr ", Let's get started!")) - ((< *elapsed-time* 910) ;; 15 min - (setq presubstr "Gooood! " - postsubstr ", Go ahead!")) - ((< *elapsed-time* 1820) ;; 30 min - (setq presubstr "So Nice! " - postsubstr ", Go ahead!")) - ((< *elapsed-time* 2730) ;; 45 min - (setq presubstr "Fantastic! " - postsubstr ", Keep going!")) - ((< *elapsed-time* 3640) ;; 60 min - (setq presubstr "Amazing! " - postsubstr ", I'm not tired!")) - (t - (setq presubstr "Awesome! " - postsubstr ", Got some rest?"))) + (if *enable* + (progn + (setq *user-name* (ros::get-param "/active_user/launch_user_name") + *elapsed-time* (ros::get-param "/active_user/elapsed_time")) + (ros::ros-debug "user -> ~A, time -> ~A (~A) " + *user-name* *elapsed-time* *target-second*) + ;; tweet depend on working time + (when (> *elapsed-time* *target-second*) + (incf *target-second* *tweet-second* ) + (ros::ros-info "tweet ~A ~A" *user-name* *elapsed-time*) + (let ((mainstr (format nil "~A has used ~A for ~d minutes" + ;; why delete *user-name* after space ? + ;;(subseq *user-name* 0 + ;;(or (position #\space *user-name*) + ;;(length *user-name*))) + *user-name* + *robot-name* + (round (/ *elapsed-time* 60)))) + presubstr postsubstr) + (cond + ((< *elapsed-time* 600) ;; 5 min + (setq presubstr "Congratulations! " + postsubstr ", Let's get started!")) + ((< *elapsed-time* 910) ;; 15 min + (setq presubstr "Gooood! " + postsubstr ", Go ahead!")) + ((< *elapsed-time* 1820) ;; 30 min + (setq presubstr "So Nice! " + postsubstr ", Go ahead!")) + ((< *elapsed-time* 2730) ;; 45 min + (setq presubstr "Fantastic! " + postsubstr ", Keep going!")) + ((< *elapsed-time* 3640) ;; 60 min + (setq presubstr "Amazing! " + postsubstr ", I'm not tired!")) + (t + (setq presubstr "Awesome! " + postsubstr ", Got some rest?"))) - (tweet-string (format nil "~A~A~A" presubstr mainstr postsubstr) - :warning-time 1 :with-image t - :volume *volume*) - - )) + (tweet-string (format nil "~A~A~A" presubstr mainstr postsubstr) + :warning-time 1 :with-image t + :speak *speak-enable* + :volume *volume*) + )))) (ros::spin-once) (ros::sleep) )