3d-to-2d-hironxjsk.l

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-utils.l")
(if (not (boundp '*robot*))
    (setq *robot* (hironxjsk)))
(send *robot* :look-at-hand :larm)
(send *robot* :stop-grasp :larm)
(setq *table* (make-cube 800 1200 5 :pos #f(800 0 0))) (send *table* :set-color :white)
(setq *ray-target* (make-sphere 20)) (send *ray-target* :set-color :blue)
(setq *tabletop-target* (make-cylinder 5 100)) (send *tabletop-target* :set-color :green)
(objects (list *robot* *ray-target* *tabletop-target* *table*))
(setq *camera* (send *robot* :camera :head_camera/rgb))
(if (not (boundp '*camera-view*))
    (setq *camera-view* (send *camera* :create-viewer)))
(setq uv-list nil)
(dotimes (i 30)
  ;; move robot head
  (send *robot* :head :neck-y :joint-angle -1 :relative t)
  (send *robot* :head :neck-p :joint-angle 0.4 :relative t)
  ;; camera/screen operations
  ;;  : screen-point  ;; "Returns point in screen corresponds to the given pos."
  (setq uv (send *camera* :screen-point (send *robot* :larm :end-coords :worldpos)))
  ;;  : "Returns ray vector of given x and y."
  (setq ray (send *camera* :ray 480 120)) ;; "Returns ray vector of given x and y."
  (setq ray-line (make-line (send *camera* :worldpos) (v+ (send *camera* :worldpos) (scale 1000 ray))))
  (send *ray-target* :locate (v+ (send *camera* :worldpos) (scale 1000 ray)) :world)
  ;;  calc intersection of table and ray-line
  (setq face-top (send *table* :face 1))
  (setq intersect-param (send face-top :intersection (send ray-line :pvertex) (send ray-line :nvertex)))
  (send *tabletop-target* :locate (send ray-line :point intersect-param) :world)
  ;; draw 3d info
  (send *camera* :draw-objects (list *robot* *ray-target* *tabletop-target*))
  (send *irtviewer* :draw-objects :flush nil)
  (send *camera-view* :viewsurface :makecurrent) ;; point-size requre this
  ;; draw camera/screen operations
  ;;  :screen-point
  (send *camera-view* :viewsurface :point-size 10)
  (send *camera-view* :viewsurface :color #f(1 0 0))
  (send *camera-view* :viewsurface :draw-point uv)
  ;;  draw history of :screen-point
  (send *camera-view* :viewsurface :point-size 2)
  (dolist (tmp-uv uv-list) (send *camera-view* :viewsurface :draw-point tmp-uv))
  (send *camera-view* :viewsurface :color #f(1 1 1))
  (send *camera-view* :viewsurface :string (elt uv 0) (elt uv 1) (format nil "larm ~A" uv))
  (push uv uv-list)
  ;;  :ray
  (send *camera-view* :viewsurface :point-size 10)
  (send *camera-view* :viewsurface :color #f(1 1 1))
  (send *camera-view* :viewsurface :draw-point #f(480 120)) ; see *camera* :ray for #f(480 120)
  (send *camera-view* :viewsurface :string 480 120 "ray point")
  ;;  :3d-point
  ;; draw sensor/kinematics info
  (send *irtviewer* :viewer :viewsurface :makecurrent)
  (send ray-line :draw-on :flush nil :color #f(0 0 1) :width 5)
  (send *camera* :draw-on :flush nil)
  (send *robot* :larm :end-coords :draw-on :flush t)
  ;; flush
  (send *camera-view* :viewsurface :flush)
  (send *irtviewer* :viewer :viewsurface :flush)
  (x::window-main-one)
  )