follow_me test program

follow_me(fail)

#!/usr/bin/env roseus

(ros::roseus "follow_hand_pulled")
(ros::rate 5)

(load "package://peppereus/pepper-interface.l")
(unless (boundp '*ri*)
  (pepper-init))
(send *ri* :angle-vector (send *pepper* :angle-vector #f(0.0 0.0 0.0 0.0 0.5 0.\
0 -0.5 0.0 0.0 -0.5 0.0 0.5 0.0 0.0 0.0)))
(send *ri* :servo-on)
(send *ri* :angle-vector (send *pepper* :angle-vector))
(setq p1 (send *pepper* :larm :end-coords :copy-worldcoords))
(setq *theta* 0)

(defun calc-pulled-vector ()
  (let (vec p2 avs-now)
    (setq p1 (send *pepper* :larm :end-coords :copy-worldcoords));; not necessa\
ry                                                                              
    (send *pepper* :angle-vector (send *ri* :state :potentio-vector))
    (setq p2 (send *pepper* :larm :end-coords :copy-worldcoords))
    (setq vec (v- (send p2 :worldpos) (send p1 :worldpos)))
    (ros::ros-info "vec: ~A" vec)
    vec))

(defun main ()
  ;;(send *robot* :angle-vector *commanded-av*)                                 
  (while t
    (ros::spin-once)
    (let* ((vec (calc-pulled-vector))
           (vec-proj (float-vector (elt vec 0) (elt vec 1) 0))
           EF-cds sleep_time)
      (if (and (> (abs (elt vec-proj 0)) 1)  (> (abs (elt vec-proj 1)) 1))
          (progn
            (setq x (elt vec-proj 0))
            (setq y (elt vec-proj 1))
            (setq *theta* (atan y x))
            (setq *theta* (* *theta* (/ 180 pi)))
            )
        (progn
          (setq *theta* 0)
          )
        )
      (ros::ros-info "~A" *theta*)
      (send *ri* :go-pos 0 0 *theta*)
      (unix:sleep 1)
      ;;(unix:sleep 5)                                                          
      ;;(send *robot* :move-to (make-coords))                                   
      (ros::sleep))))