ロボット実機とシミュレータを同時に動かせる*rs*を生成するスクリプト

init.l

(load "robot-server-simulator.l")
(load "../client/eus-client/robot-client2.l")

(defclass robot-server-wrapper)
(defmethod robot-server-wrapper
  (:init
    ()
    self)
  (:nomethod (&rest args)
    (let (ret)
      ;;(warn "called ~A, ~A~%" (car args) (cdr args)))
      ;;(prin1 args)
      (when (boundp '*rs-sim*)
        (setq ret (send* *rs-sim* args))) 
      (when (boundp '*rs-real*)
        (setq ret (send* *rs-real* args)))
      ret
      )
    )
  )

(defclass vision-server-wrapper)
(defmethod vision-server-wrapper
  (:init
    ()
    self)
  (:nomethod (&rest args)
    (let (ret)
      ;;(warn "called ~A, ~A~%" (car args) (cdr args)))
      ;;(prin1 args)
      (when (boundp '*vs-sim*)
        (setq ret (send* *vs-sim* args))) 
      (when (boundp '*vs-real*)
        (setq ret (send* *vs-real* args)))
      ret
      )
    )
  )

(defun init-robot
  (&key (real nil) (sim t))
  (makunbound '*rs-real*)
  (makunbound '*rs-sim*)
  (if real
    (progn
      (setq *rs-real* (connect-robot-server))
      (warn "create real robot~%")))
  (if sim
    (progn
      (setq *rs-sim* (connect-robotsim-server))
      (warn "create sim~%")))
  (setq *rs* (instance robot-server-wrapper :init))
  (reset-robot)
  )

(defun reset-robot
  ()
  (send *rs* :wheel-reset)
  (send *rs* :daisya-controlmode 1)
  (send *rs* :daisya-velocity-vector #f(0 0 0))

  (send *rs* :arm-angle-vector #f(0 0 0 0 0))
  (send *rs* :arm-angle-vector #f(0 0 0 0 0) 1000)
  (send *rs* :arm-poweron-vector #f(0 0 0 0 1))
  (send *rs* :arm-angle-vector #f(0 0 0 0 0) 200)
  (unix:usleep (* 200 1000))
  (send *rs* :arm-poweron-vector #f(0 0 0 1 1))
  (unix:usleep (* 200 1000))
  (send *rs* :arm-poweron-vector #f(0 0 1 1 1))
  (unix:usleep (* 200 1000))
  (send *rs* :arm-poweron-vector #f(0 1 1 1 1))
  (unix:usleep (* 200 1000))
  (send *rs* :arm-poweron-vector #f(1 1 1 1 1))
  (unix:usleep (* 100 1000))
  )

(defun init-vision
  (&key (real nil) (sim t))
  (makunbound '*vs-real*)
  (makunbound '*vs-sim*)
  (if real
    (progn
      (setq *vs-real* (connect-vision-server))
      (warn "create real robot~%")))
  (if sim
    (progn
      (setq *vs-sim* (connect-visionsim-server))
      (warn "create sim~%")))
  (setq *vs* (instance vision-server-wrapper :init))
)

(defun stop-robot
  ()
  (send *rs* :arm-angle-vector #f(0 0 0 0 0) 1000)
  (send *rs* :wheel-reset)
  (send *rs* :daisya-controlmode 1)
  (send *rs* :daisya-velocity-vector #f(0 0 0))
  (unix:sleep 1)
  (send *rs* :arm-poweron-vector #f(0 0 0 0 0))
  )

(warn "----init.l loaded----~%")
(warn "sim: (init-robot)~%")
(warn "sim + real: (init-robot :real t)~%")
(warn "real: (init-robot :real t :sim nil)~%")
(warn "sim: (init-vison)~%")
(warn "sim + real: (init-vision :real t)~%")
(warn "real: (init-vision :real t :sim nil)~%")