;;;
;;; Copyright (c) 2010, Lorenz Moesenlechner <moesenle@in.tum.de>
;;; All rights reserved.
;;; 
;;; Redistribution and use in source and binary forms, with or without
;;; modification, are permitted provided that the following conditions are met:
;;; 
;;;     * Redistributions of source code must retain the above copyright
;;;       notice, this list of conditions and the following disclaimer.
;;;     * Redistributions in binary form must reproduce the above copyright
;;;       notice, this list of conditions and the following disclaimer in the
;;;       documentation and/or other materials provided with the distribution.
;;;     * Neither the name of the Intelligent Autonomous Systems Group/
;;;       Technische Universitaet Muenchen nor the names of its contributors 
;;;       may be used to endorse or promote products derived from this software 
;;;       without specific prior written permission.
;;; 
;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
;;; POSSIBILITY OF SUCH DAMAGE.
;;;

(in-package :ct)

(defvar *spine-action-client* nil)

(defun init-spine ()
  (setf *spine-action-client*
        (actionlib:make-action-client "/torso_controller/position_joint_action"
                                      "pr2_controllers_msgs/SingleJointPositionAction"))
  (actionlib:wait-for-server *spine-action-client*))

(register-ros-init-function init-spine)

(defun move-spine (pos)
  "Moves the spine to the position indicated by `pos'. Terminates the
  action if it got stuck."
  (let ((velocity-fl (make-fluent :name 'velocity-fl :value nil)))
    (flet ((feedback-cb (msg)
             (with-fields (velocity)
                 msg
               (setf (value velocity-fl) velocity))))
      (pursue
        (call-action *spine-action-client*
                     (make-msg "pr2_controllers_msgs/SingleJointPositionGoal"
                               position pos
                               max_velocity 1)
                     :feedback-cb #'feedback-cb)
        (seq
          (wait-for velocity-fl)
          (wait-for (> velocity-fl 0.01) :timeout 5.0)
          (wait-for (< velocity-fl 0.005)))))))