;;; -*- Mode: Lisp; Syntax: Common-Lisp; Package: Hamsterdam -*-
;;;; The linkage from Harlequin LispWorks to the TCP socket that talks to a
;;;; specially-modified version of Hamsterdam.
;;; Note that only the really LispWorks-specific forms are conditionalized.
;;; I could avoid conditionalizing any of them by making the inclusion of the
;;; entire file in the sysdcl conditional upon which platform is in use, but
;;; then it'll be hard to distribute the system (e.g., to the UNIX filesystem
;;; via SCT:EXPORT-SYSTEM et al), and so forth.
(in-package :hamsterdam)
;;;; Linkage to the required C code.
;;; This should give a warning that "the following new foreign symbols are
;;; unresolved: lisp_parse_turn ... etc". It can apparently be safely ignored.
;;; &&& This needs to turn into the appropriate loading-form in the sysdcl
;;; &&& instead, both so the absolute pathname here is removed, and so we
;;; &&& can track it in the sysdcl. However, that can't happen until I look
;;; &&& up the HCL syntax for how to do that.
#+LispWorks
(eval-when (load)
(ffi:read-foreign-modules "Alive/ServerExamples/lisp-end.o"))
;;;; Foreign interfaces.
#-LispWorks
(defmacro-definer DEF-LISPWORKS-FOREIGN-FUNCTION ((lisp-fn &rest ignore)
&rest ignore)
`(defun ,lisp-fn (&rest ignore)
(unimplemented "This is the Lisp side of a Harlequin LispWorks foreign-function call.~&~
If you are seeing this error, then something is trying to call it, even~&~
though the current platform is not Harlequin LispWorks.")))
#+LispWorks
(defmacro-definer DEF-LISPWORKS-FOREIGN-FUNCTION (&body definition)
`(foreign:define-foreign-function ,@definition))
(def-lispworks-foreign-function (open-socket "ALIVE_open_socket")
()
:result-type :uinteger ; Success: 0. Failure: 1.
:language :ansi-c)
(def-lispworks-foreign-function (close-socket "ALIVE_close_socket")
()
:result-type :integer ; Success: 0. Failure: -1 (errno contains failure code).
:language :ansi-c)
(def-lispworks-foreign-function (write-socket "ALIVE_write_socket")
((string :simple-string))
:result-type :integer ; Success: number of bytes written. Failure: -1 (errno contains failure code).
:language :ansi-c)
(def-lispworks-foreign-function (read-socket "ALIVE_read_socket")
((string :simple-string))
:result-type :integer ; Success: number of bytes read. Failure: -1 (errno contains failure code).
:language :ansi-c)
;;;; Basic communication with the socket.
(defparameter *BUFFER-NULL-CHARACTER* #\Space)
(defun STUFF-BUFFER (buffer format-string &rest format-args)
(let ((result (apply #'format nil format-string format-args)))
(when (> (length result) (length buffer))
(error "The result ~S~&(of length ~D) won't fit in the buffer, which is of length ~D."
result (length result) (length buffer)))
(fill buffer *buffer-null-character*) ; Clear out any trailing gubbish.
(replace buffer result))) ; Stick in the new command.
(defparameter *SOCKET-BUFFER-LENGTH* 256)
(defun MAKE-SOCKET-BUFFER ()
(make-string *socket-buffer-length* :initial-element #\Space))
(defmacro WITH-SOCKET (&body body)
`(unwind-protect
(progn
(open-socket)
,@body)
(close-socket)))
(defparameter *GRACEFUL-TERMINATION-OPCODE* "+++TEARDOWN+++")
(defun STUFF-BUFFER-AND-WRITE (buffer format-string &rest format-args)
(apply #'stuff-buffer buffer format-string format-args)
(write-socket buffer))
(defmacro WITH-SOCKET-GRACEFULLY-TERMINATED (&body body)
`(with-socket
(unwind-protect
(progn
,@body)
(let ((command (make-socket-buffer)))
(stuff-buffer-and-write command "~A" *graceful-termination-opcode*)))))
(defmacro WITH-BUFFERS-AND-SOCKET ((in out) &body body)
`(let ((,out (make-socket-buffer)) ; Strictly speaking, these could be the same buffer, ...
(,in (make-socket-buffer))) ; ... but this makes debugging easier.
(with-socket-gracefully-terminated
,@body)))
;;;; Test command loops and exercisers.
(defun COMMAND-LOOP ()
(with-buffers-and-socket (in out)
(let ((command nil))
(catch 'done
(loop do (format t "Command? ")
(setf command (read-line))
(when (string-equal command ".")
(throw 'done nil))
(stuff-buffer-and-write out "~A" command)
(read-socket in)
(format t "~&Sensor echo: ~S~&" in)))))
(values))
(defun SEND-COMMAND-IGNORING-ECHO (verbose in out format-string &rest format-args)
(when verbose
(apply #'format t format-string format-args)
(terpri))
(apply #'stuff-buffer-and-write out format-string format-args)
(read-socket in)
(values))
(defun SPIN-THE-PUPPET (theta-low theta-high &optional (verbose t))
(with-buffers-and-socket (in out)
(loop for apxsecs from 1 to 30
do (loop for theta from theta-low below theta-high by (/ (- theta-high theta-low) 10)
do (send-command-ignoring-echo
verbose in out "STAND ~F 1 ~F 1.5"
apxsecs theta)
(send-command-ignoring-echo
verbose in out "STAND ~F 1 0 1.5"
apxsecs))))
(values))
;;;; Interfaces from basic actions to the communications substrate.
;;; All of these take output (for outgoing commands) and input (from sensor updates) buffers,
;;; and also whatever parameters the individual command requires. They also take an optional
;;; debugging arg, which prints out what they're doing. None of them return any value.
;;; +++ Internal stuff.
(defun CREATE-AND-MAYBE-ECHO-COMMAND (verbose format-string &rest format-args)
(let ((command (apply #'format nil format-string format-args)))
(when verbose
(format t "~A~&" command))
command))
(defparameter *SHOW-SENSOR-ECHO* t)
(defun COMMAND-AND-SENSE (in out verbose format-string &rest format-args)
(let ((command (apply #'create-and-maybe-echo-command verbose format-string format-args)))
(stuff-buffer-and-write out "~A" command)
(let* ((buffer-length (read-socket in)) ; This should probably always be 256 or whatever the length of the buffer is...
(real-length (or (position #\Null in)
buffer-length))) ; In case it was totally stuffed, with no room for null termination (at least, we _hope_ that's why!...)
(when *show-sensor-echo*
(format t "~&Sensor echo:~&~A~&"
(subseq in 0 real-length)))))
(values))
(defun CONTYPE->ENUM (contype)
(ecase contype
(:pelvis 0)
(:head 1)
(:lhand 2)
(:rhand 3)
(:lfoot 4)
(:rfoot 5)))
;;; +++ External stuff.
(defun NOOP (in out verbose)
(command-and-sense in out verbose "NOOP"))
(defun STAND (in out time-to-completion desired-theta velocity &optional verbose)
(command-and-sense in out verbose "STAND ~D 1 ~D ~D" time-to-completion desired-theta velocity))
(defun SIT (in out velocity &optional verbose)
(command-and-sense in out verbose "SIT ~D" velocity))
(defun SQUAT (in out velocity &optional verbose)
(command-and-sense in out verbose "SQUAT ~D" velocity))
(defun TURN (in out x y z angular-velocity &optional verbose)
(command-and-sense in out verbose "TURN ~D ~D ~D ~D" x y z angular-velocity))
(defun WAVE (in out contype velocity &optional (how-many-times 1) verbose)
(command-and-sense in out verbose "WAVE ~D ~D ~D" how-many-times velocity (contype->enum contype)))
(defun POINT (in out x y z velocity &optional (how-many-times 1) verbose)
(command-and-sense in out verbose "POINT ~D ~D ~D ~D ~D" x y z velocity how-many-times))
(defun WALK-TO-SPOT (in out x y z spin-duration velocity &optional verbose)
(command-and-sense in out verbose "WALK_TO_SPOT ~D ~D ~D ~D ~D" x y z spin-duration velocity))
(defun JUMP (in out drop-time leap-time how-high &optional verbose)
(command-and-sense in out verbose "JUMP ~D ~D ~D" drop-time leap-time how-high))
(defun GRIN (in out &optional verbose)
(command-and-sense in out verbose "GRIN"))
(defun SAD (in out &optional verbose)
(command-and-sense in out verbose "SAD"))
;;; +++ A large test of the above.
(defun TEST-COMMANDS (&optional (verbose t))
(with-buffers-and-socket (in out)
(grin in out verbose)
(stand in out 10 10 1.5 verbose)
(sit in out 15 verbose)
(stand in out 1 0 1 verbose) ; For some weird reason, SQUAT right after SIT takes practically forever!
(squat in out 50 verbose)
(turn in out -100 0 -150 1.5 verbose)
(wave in out :lhand 15 3 verbose)
(point in out 0 150 -150 15 1 verbose)
(jump in out 100 100 30 verbose)
(sad in out verbose)
(sleep 2) ; So we can see the frown.
(grin in out verbose))
(values))
(defun TEST-WALKING (&optional (verbose t))
(with-buffers-and-socket (in out)
(loop with done? = nil
until done?
for coords = (progn (format t "~&Walk to where? (x y z dur vel) ")
(read)) ; Yuck.
do (cond ((consp coords)
(destructuring-bind (x y z dur vel) coords
(walk-to-spot in out x y z dur vel verbose)))
(t
(format t "~&Exiting...~&")
(setf done? t))))))
;;; End of file.