;;;
;;; 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 *attach-bounding-box* nil)
(defvar *move-arm-relative* nil)
(defvar *move-arm-to-point* nil)
(defvar *left-gripper* nil)
(defvar *right-gripper* nil)
(defvar *tuck-arms* nil)

(define-condition no-ik-solution (plan-error) ())
(define-condition ik-link-in-collision (plan-error) ())
(define-condition start-state-in-collision (plan-error) ())
(define-condition goal-state-in-collision (plan-error) ())
(define-condition trajectory-controller-failed (plan-error) ())

(defun init-grasping ()
  "Initializes the grasping action clients and waits for them to come up"
  (setf *attach-bounding-box*
        (actionlib:make-action-client
         "/attach_bounding_box" "cotesys_ros_grasping/AttachBoundingBoxAction"))
  (setf *move-arm-relative*
        (actionlib:make-action-client
         "/move_arm_relative_cartesian_point"
         "cotesys_ros_grasping/MoveArmRelativeCartesianPointAction"))
  (setf *move-arm-to-point*
        (actionlib:make-action-client
         "/move_arm_to_position"
         "cotesys_ros_grasping/MoveArmToPositionAction"))
  (setf *left-gripper*
        (actionlib:make-action-client
         "/l_gripper_controller/gripper_action"
         "pr2_controllers_msgs/Pr2GripperCommandAction"))
  (setf *right-gripper*
        (actionlib:make-action-client
         "/r_gripper_controller/gripper_action"
         "pr2_controllers_msgs/Pr2GripperCommandAction"))
  (setf *tuck-arms*
        (actionlib:make-action-client
         "/tuck_arms"
         "pr2_common_action_msgs/TuckArmsAction"))
  (top-level
    (par
      (actionlib:wait-for-server *attach-bounding-box*)
      (actionlib:wait-for-server *move-arm-relative*)
      (actionlib:wait-for-server *move-arm-to-point*)
      (actionlib:wait-for-server *left-gripper*)
      (actionlib:wait-for-server *right-gripper*)
      (actionlib:wait-for-server *tuck-arms*))))
(register-ros-init-function init-grasping)

(defun park-arm (side &optional (grasp "default"))
  "Moves the arms to a 'parking position' on the side so that they are
out of the way and not be seen by navigation.

`side' is either :left or :right, indicating the arm to be parked.

`grasp' is the name of the grasp, indicating the gripper
orientation. It defaults to 'default' but when carrying an object, a
different value might be used."
  (call-action
   *move-arm-to-point*
   (make-message "cotesys_ros_grasping/MoveArmToPositionGoal"
                 arm_name (ecase side
                            (:left "left_arm")
                            (:right "right_arm"))
                 grasp_name grasp
                 (x point) 0.2
                 (y point) (ecase side
                             (:left 0.6)
                             (:right -0.6))
                 (z point) 0.9)))

(defun tuck-arm (side)
  "Executes the tuck_arms action for the corresponding arm.

`side' is either :left, :right or :both, indicating the arm to be
tucked. The other arm will be untucked."
  (call-action *tuck-arms* (make-msg "pr2_common_action_msgs/TuckArmsGoal"
                                     tuck_left (case side
                                                 (:left t)
                                                 (:both t)
                                                 (t nil))
                                     tuck_right (case side
                                                  (:right t)
                                                  (:both t)
                                                  (t nil)))))

(defun open-gripper (side)
  "Opens the gripper. Blocks until the gripper is really open.

`side' is either :left or :right, indicating the gripper to be opened."
  (call-action
   (ecase side
     (:left *left-gripper*)
     (:right *right-gripper*))
   (make-message "pr2_controllers_msgs/Pr2GripperCommandGoal"
                 (position command) 0.08
                 (max_effort command) 1000)))

(defun close-gripper (side)
  "Closes the gripper and returns when the gripper doesn't move
anymore. Please note that the action keeps running since preemtion of
the message would mean that the gripper would not apply any force and
all carried objects would fall out.

`side' is either :left or :right, indicating the gripper to be closed."
  (let ((last-position-fl (make-fluent :name 'last-position :value nil))
        (done-fl (make-fluent :name 'done-fl :value nil)))
    (flet ((feedback-cb (feedback)
             (setf (value last-position-fl)
                   (pr2_controllers_msgs-msg:position-val feedback)))
           (done-cb (state result)
             (declare (ignore state result))
             (setf (value done-fl) t)))
      (actionlib:send-goal
       (ecase side
         (:left *left-gripper*)
         (:right *right-gripper*))
       (make-message "pr2_controllers_msgs/Pr2GripperCommandGoal"
                     (position command) 0.0
                     (max_effort command) 1000)
       #'done-cb #'feedback-cb)      
      (pursue
        (wait-for done-fl)
        (loop
          for pos = (value last-position-fl)
          do (wait-for (pulsed last-position-fl
                               :handle-missed-pulses :never))
          until (and pos (< (abs (- pos (value last-position-fl)))
                                 0.0001)))))))

(defun handle-action-failure (f)
  (case (motion-planning-error-code (cotesys_ros_grasping-msg:error_code-val
                                     (result f)))
    (:no_ik_solution (fail 'no-ik-solution))
    (:ik_link_in_collision (fail 'ik-link-in-collision))
    (:start_state_in_collision (fail 'start-state-in-collision))
    (:goal_in_collision (fail 'goal-state-in-collision))
    (:trajectory_controller_failed (fail 'trajectory-controller-failed))
    (t (fail f))))

(defun move-arm-to-point (side point grasp)
  "Calls the /move_arm_to_position action. 

`side' is either :left or right, indicating the arm to be moved

`point' is a CL-TF:POINT-STAMPED indicating the goal point

`grasp' is a string containing the name of the grasp as defined in the
cotesys_ros_grasping config"
  (declare (type cl-tf:point-stamped point)
           (type string grasp))
  (let ((pt-in-base (tf:transform-point
                     *tf* :target-frame "/base_link"
                     :point point)))
    (handler-case
        (call-action
         *move-arm-to-point*
         (make-message "cotesys_ros_grasping/MoveArmToPositionGoal"
                       arm_name (ecase side
                                  (:left "left_arm")
                                  (:right "right_arm"))
                       grasp_name grasp
                       (x point) (cl-transforms:x pt-in-base)
                       (y point) (cl-transforms:y pt-in-base)
                       (z point) (cl-transforms:z pt-in-base)))
      (action-failure (f)
       (handle-action-failure f)))))

(defun move-arm-relative (side vector)
  "Calls the /move_arm_relative_cartesian_point action.

`side' is either :left or right, indicating the arm to be moved

`vector' is a CL-TRANSFORMS:3D-VECTOR, indicating the relative goal
position the arm should move to"
  (declare (type cl-transforms:3d-vector vector))
  (handler-case
      (call-action
       *move-arm-relative*
       (make-message "cotesys_ros_grasping/MoveArmRelativeCartesianPointGoal"
                     arm_name (ecase side
                                (:left "left_arm")
                                (:right "right_arm"))
                     (x rel_point) (cl-transforms:x vector)
                     (y rel_point) (cl-transforms:y vector)
                     (z rel_point) (cl-transforms:z vector)))
    (action-failure (f)
      (handle-action-failure f))))

(defun attach-bounding-box (side obj)
  "Calls the /attach_bounding_box action to attach an object to the gripper.

`side' is either :left or right, indicating the side the object should
be attached to

`obj' is the CollisionObject returned by the perception routine
"
  (declare (type mapping_msgs-msg::<CollisionObject> obj))
  (call-action
   *attach-bounding-box*
   (make-message "cotesys_ros_grasping/AttachBoundingBoxGoal"
                 arm_name (ecase side
                              (:left "left_arm")
                              (:right "right_arm"))
                 object obj
                 remove nil)))

(defun detach-bounding-box (side obj)
  "Calls the /attach_bounding_box action to remove the object from the gripper.

`side' is either :left or right, indicating the side the object should
be removed from
"
  (declare (type mapping_msgs-msg::<CollisionObject> obj))  
  (call-action
   *attach-bounding-box*
   (make-message "cotesys_ros_grasping/AttachBoundingBoxGoal"
                 arm_name (ecase side
                              (:left "left_arm")
                              (:right "right_arm"))
                 object obj
                 remove t)))

