Skip to content

Commit

Permalink
add enable and speak-enable dynamic reconfigure
Browse files Browse the repository at this point in the history
  • Loading branch information
@knorth55
knorth55 committed Nov 18, 2022
1 parent 2479f0e commit 515a8d8
Show file tree
Hide file tree
Showing 4 changed files with 196 additions and 133 deletions.
33 changes: 25 additions & 8 deletions jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_tablet.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
110 changes: 63 additions & 47 deletions jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_uptime.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)
82 changes: 48 additions & 34 deletions jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_warning.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down
104 changes: 60 additions & 44 deletions jsk_robot_common/jsk_robot_startup/lifelog/tweet_client_worktime.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)

0 comments on commit 515a8d8

Please sign in to comment.