Skip to content

[WIP] add WIP contents for pickle.l #755

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
k-okada wants to merge 1 commit into
base: master
Choose a base branch
from
Open

[WIP] add WIP contents for pickle.l #755

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 33 additions & 0 deletions roseus_smach/src/check-pickle.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
(load "pickle.l")
(require :base64 "lib/llib/base64.l")

(dolist (data `(
(t . "STAxCi4=")
(nil . "STAwCi4=")
(nil . "Ti4=")
("hello" . "UydoZWxsbycKcDAKLg==")
(10 . "STEwCi4=")
(12.3 . "RjEyLjMKLg==")
(,(format nil "Hello~%World") . "UydIZWxsb1xuV29ybGQnCnAwCi4==")
((1 2 3) . "KGxwMApJMQphSTIKYUkzCmEu")
((1 12.3 "Hello" t nil) . "KGxwMApJMQphRjEyLjMKYVMnSGVsbG8nCnAxCmFJMDEKYUkwMAphLg==")
((1 2 3 (4 5)) . "KGxwMApJMQphSTIKYUkzCmEobHAxCkk0CmFJNQphYS4=")
((1 2 3 ("Hello" "World")) . "KGxwMApJMQphSTIKYUkzCmEobHAxClMnSGVsbG8nCnAyCmFTJ1dvcmxkJwpwMwphYS4=")
;; KGRJMTIzClMnSGVsbG8nCnNTJ1dvcmxkJwpGMTIuMwpzLg==
(((0 . "zero") ("Hello" . "World")) . "KGRwMApJMApTJ3plcm8nCnAxCnNTJ0hlbGxvJwpwMgpTJ1dvcmxkJwpwMwpzLg==")
))
(print data)
(let (str obj)
(setq str (user::base64decode (cdr data)))
(setq obj (pickle::load str))
;(print "describe 1")
;(describe (car data))
;(print "describe 2")
;(describe obj)
(warning-message 2 "check ~A == ~A (from ~A ~A)~%" (car data) obj str (cdr data))
(unless (equal (car data) obj)
(warning-message 1 "pickle load/save failed [~A] -> ~A~%" obj (equal (car data) obj)))
))
(exit)


17 changes: 17 additions & 0 deletions roseus_smach/src/p.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
import base64
import pickle

#print(base64.b64decode("gAJ9cQAu"))
#print(pickle.loads(base64.b64decode("gAJ9cQAu"))) # -> {}

objs = []
for msg in [True, False, None, 'hello', 10, 12.3, "Hello\nWorld", [1, 2, 3], [1, 12.3, "Hello", True, False], {0: "zero", "Hello": "World"} ]:
obj = base64.b64encode(pickle.dumps(msg,0))
print("({} . \"{}\")".format(msg, obj))
objs.append(obj)
#print(msg.decode('utf-8'))
# print(base64.b64decode(msg))
# print(pickle.loads(base64.b64decode(msg)))

for obj in objs:
print("{} -> {}".format(base64.b64decode(obj), pickle.loads(base64.b64decode(obj))))
249 changes: 249 additions & 0 deletions roseus_smach/src/pickle.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,256 @@
(format ss "~c" _SETITEM)))
)


(defclass unpickler
:super propertied-object
:slots (eof stack mark))
(defmethod unpickler
(:init
(&key ((:dict dct) nil))
(setq eof (gentemp "EOF"))
(setq mark (gentemp "MARK"))
(setq stack nil))
(:read
(obj &optional (strm nil))
(let (ret)
(labels
((dictp (x) (and (consp x) (= (length x) 1) (cdr x) (atom (cdr x)) (car x)))
(deep-reverse (x) (map cons #'(lambda (x) (cond ((dictp x) (cons (cdr x) (if (consp (car x)) (deep-reverse (car x)) (car x))))
((consp x) (deep-reverse x))
(t x)))
(reverse x)))
)
#+:debug
(progn
(format t "reading [")
(dotimes (i (length obj))
(format t "~d(~c) " (elt obj i) (elt obj i)))
(format t "]~%"))
(setq ret (send self :load (make-string-input-stream obj)))
(if (consp ret)
(deep-reverse ret)
ret)
)))
(:load
(is)
(let (c)
(catch :STOP
(while (not (eq (setq c (read-char is nil eof)) eof))
;; (format t "LOAD stack:~A (~c)~%" stack c)
(case c
((#._INT #\L)
(send self :load-int is))
(#._FLOAT
(send self :load-float is))
(#._STRING
(send self :load-string is))
(#\J ;; BININT push four-byte signed int
(send self :load-binint is))
(#\K ;; BININT1 push 1-byte unsigned int
(send self :load-binuint is 1))
(#\M ;; BININT2 push 2-byte unsigned int
(send self :load-binuint is 2))
(#x8a ;; LONG1 push long from < 256 bytes
(send self :load-binuint is (read-char is nil eof)))
(#\G ;; BINFLOAT push float; arg is 8-byte float encoding
(send self :load-binfloat is))
((#\T #\U) ;; BINSTRING, SHORT_BINSTRING push string; counted binary string argument
(send self :load-binstring is))
(#\N ;; NONE push None
(send self :load-none is))
(#\p ;; PUT store stack top in memo; index is string arg
(while (/= (setq c (read-char is nil eof)) #\newline)))
(#\q ;; BINPUT store stack top in memo; index is 1-byte arg
(read-char is nil eof))
(#x88 ;; NEWTRUE (136) push True
(push t stack))
(#x89 ;; NEWFALSE (137) push False
(push nil stack))
(#._MARK
(send self :load-mark is))
((#._LIST #\t) ;; TUPLE build tuple from topmost stack items
(send self :load-list is))
((#\] #\}) ;; EMPTY_LIST push empty list, EMPTY_DICT push empty dict
(push nil stack))
(#._DICT
(send self :load-dict is))
(#._SETITEM
(send self :load-setitem is))
(#\u
(send self :load-setitems is))
(#._APPEND
(send self :load-append is))
(#\e ;; APPENDS extend list on stack by topmost stack slice
(send self :load-appends is))
(#._STOP
(send self :load-stop is))
(#x80 ;; PROTO identify pickle protocol
(read-char is nil eof)) ;; protocol version
(#x85 ;; TUPLE1 build 1-tuple from three topmost stack items
(send self :load-append-n is 1))
(#x86 ;; TUPLE2 build 2-tuple from three topmost stack items
(send self :load-append-n is 2))
(#x87 ;; TUPLE3 build 3-tuple from three topmost stack items
(send self :load-append-n is 3))
(t
(warning-message 1 "Unknown string ~c(~d)~%" c c)))))))
(:load-int
(is)
(let (c str (ss (make-string-output-stream 4096)))
(while (/= (setq c (read-char is nil eof)) #\newline)
(format ss "~c" c))
(setq str (get-output-stream-string ss))
(push
(cond ((string= str "00") nil)
((string= str "01") t)
(t (read-from-string (string-right-trim "L" str))))
stack)))
(:load-float
(is)
(let (c (ss (make-string-output-stream 4096)))
(while (/= (setq c (read-char is nil eof)) #\newline)
(format ss "~c" c))
(push (read-from-string (get-output-stream-string ss)) stack)))
(:load-string
(is)
(let (c (ss (make-string-output-stream 4096)))
(while (/= (setq c (read-char is nil nil)) #\newline)
(case c
(#\'
(format ss "\""))
(#\\
(if (setq c (read-char is nil nil))
(case c
(#\n
(format ss "~c" #\newline))
(#\t
(format ss "~c" #\tab))
(t
(warning-message 3 "Unknown string tag \~c(~d)~%" c c)))))
(t
(format ss "~c" c))))
(push (read-from-string (get-output-stream-string ss)) stack)))
(:load-binint
(is)
(let (c (r 0))
(dotimes (i 4)
(setq c (read-char is nil eof))
(incf r (* c (expt 256 i))))
(push (if (< r #x80000000) r (- r #x100000000)) stack)))
(:load-binuint
(is n)
(let (c (r 0))
(dotimes (i n)
(setq c (read-char is nil eof))
(incf r (* c (expt 256 i))))
(push r stack)))
(:load-binfloat
(is)
(let ((buf "01234567"))
(dotimes (i 8)
(setf (elt buf (- 7 i)) (read-char is nil eof)))
(push (sys::peek buf 0 :double) stack)))
(:load-binstring
(is)
(let (c n (ss (make-string-output-stream 4096)))
(setq n (read-char is nil nil))
(dotimes (i n)
(setq c (read-char is nil nil))
(case c
(#\'
(format ss "\""))
(#\\
(if (setq c (read-char is nil nil))
(case c
(#\n
(format ss "~c" #\newline))
(#\t
(format ss "~c" #\tab))
(t
(warning-message 3 "Unknown string tag \~c(~d)~%" c c)))))
(t
(format ss "~c" c))))
(push (get-output-stream-string ss) stack)))
(:load-none
(is)
nil)
(:load-mark
(is)
(push mark stack))
(:marker
()
(let ((k 0))
(while (not (eq (elt stack k) mark))
(incf k))
k))
(:load-list
(is)
(let ((k (send self :marker)))
(setq stack (concatenate cons (list (subseq stack 0 k)) (if (< (1+ k) (length stack)) (subseq stack (1+ k)) (list ))))
))
(:load-dict
(is)
(let* ((k (send self :marker))
(items (subseq stack 0 k))
(dict nil))
(do ((j 0)) ((>= j (length items)))
(push (cons (elt items j) (elt items (1+ j))) dict)
(incf j 2))
(setq stack (concatenate cons (list dict) (if (< (1+ k) (length stack)) (subseq stack (1+ k)) (list ))))
))
(:load-setitem
(is)
(let* ((key (pop stack))
(value (pop stack)))
(push (cons key value) (car stack))))
(:load-setitems
(is)
(let (items key value)
(while (not (eq (car stack) mark))
(setq key (pop stack))
(setq value (pop stack))
(push (cons key value) items))
(nreverse items)
(pop stack) ;; remove marker
(setf (car stack) (append (car stack) items))
))
(:load-append
(is)
(let ((item (pop stack)))
(push item (car stack))
))
(:load-appends
(is)
(let (items)
(while (not (eq (car stack) mark))
(push (pop stack) items))
(nreverse items)
(pop stack) ;; remove marker
(setf (car stack) (append (car stack) items))
))
(:load-append-n
(is len)
(let (items)
(print stack)
(dotimes (i len)
(push (pop stack) items))
(nreverse items)
(push items stack)
))
(:load-stop
(is)
(throw :STOP (pop stack)))
;;
)

(shadow 'dump (find-package "PICKLE"))
(shadow 'load (find-package "PICKLE"))

(defun pickle::dump (obj &optional (strm nil))
(send (instance pickler :init :dict t) :dump obj strm ))

(defun pickle::load (obj)
(send (instance unpickler :init :dict t) :read obj))

(provide :pickle "pickle.l")
15 changes: 14 additions & 1 deletion roseus_smach/src/state-machine-ros.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
(require :state-machine-utils "package://roseus_smach/src/state-machine-utils.l")
(require :pickle "pickle.l")

(require :base64 "lib/llib/base64.l")

(ros::roseus-add-msgs "smach_msgs")

;;
Expand Down Expand Up @@ -111,6 +113,7 @@
)
(:set-initial-state-cb
(msg)
(ros::ros-error "start -cb")
(let ((path (send msg :path))
;; this smach takes only one state
(name (car (send msg :initial_states))))
Expand All @@ -119,6 +122,7 @@
(return-from :set-initial-state-cb))
(setq path (format nil "~A/~A"
(subseq path (1+ (length root-name))) name))
(ros::ros-error "path =~A init-tm " path init-tm)
;; initialize can be called every 5 sec
(when init-tm
(when (< (send (ros::time- (ros::time-now) init-tm) :to-sec) 5)
Expand All @@ -139,7 +143,12 @@
(send sm :reset-state)
(ros::ros-info "Set initial/active state [~a]" nd)))
;; TODO apply userdata
(print "------------------------")
(print (send msg :initial_states))
(print (send msg :local_data))
(print (pickle::load (base64decode (send msg :local_data))))
(setq init-tm (ros::time-now))
(ros::ros-error "start -cb done")
))
;;
;; utility for users
Expand All @@ -162,10 +171,14 @@
(send self :publish-all-status user-data))
(:exec-state-machine (&key (user-data) (reset-state))
(send self :reset-state :user-data user-data :on-state reset-state)
(ros::ros-error "start exec-state-machine")
(while (not (send sm :goal-reached))
(ros::ros-error "start exec")
(send self :spin-once)
(send self :publish-status user-data)
(send self :state-machine :execute user-data :step -1))
(send self :state-machine :execute user-data :step -1)
(ros::ros-error "start exec done")
)
(warn ";; goal reached at ~A~%" (send sm :active-state))
(send self :publish-all-status user-data)
(send sm :active-state))
Expand Down
Loading
Loading