;;;
;;; 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)

(declare-goal achieve (occasion)
  (ros-info (achieve cram-tutorial) "Achieving `~a'" occasion))

(declare-goal perceive (obj)
  (declare (ignore obj)))

(defmacro at-location (&whole sexp (loc-var) &body body)
  `(with-task-tree-node (:path-part `(goal-context (at-location (?loc)))
                         :name ,(format nil "AT-LOCATION")
                         :sexp ,sexp
                         :lambda-list (,loc-var)
                         :parameters (list ,loc-var))
     (achieve `(robot-at ,,loc-var))
     ,@body))

;;;
;;; TODO: Use at-location instead of (achieve (robot-at ?loc)) to
;;; enable reasoning about locations where actions have been performed
;;; 

(def-goal (perceive ?obj-name)
  (find-object ?obj-name))

(def-goal (achieve (robot-at ?loc))
  (ros-info (robot-at cram-tutorial) "Building collision map")
  (ros-info (robot-at cram-tutorial) "Parking arms")  
  (park-arm :left (calculate-grasp *left-gripper-obj*))
  (park-arm :right (calculate-grasp *right-gripper-obj*))
  (ros-info (robot-at cram-tutorial) "Navigating")
  (pursue
    (nav-to ?loc)
    ;; Hack to stop navigating if we got stuck
    (sleep 10)))

(def-goal (achieve (arm-at relative ?vec ?side))
  (move-arm-relative ?side ?vec))

(def-goal (achieve (arm-at absolute ?point ?grasp ?side))
  (move-arm-to-point ?side ?point ?grasp))

(def-goal (achieve (gripper-open ?side))
  (open-gripper ?side))

(def-goal (achieve (gripper-closed ?side))
  (close-gripper ?side))

(def-goal (achieve (object-in-hand ?obj-name ?side))
  (let ((retry-count 5))
    (with-failure-handling
        ((no-ik-solution (e)
           (declare (ignore e))
           (when (>= retry-count 0)
             (decf retry-count)
             (achieve `(robot-at ,(tf:make-pose-stamped
                                   "/base_link" 0.0
                                   (cl-transforms:make-3d-vector (- (random 0.2) 0.1)
                                                                 (- (random 0.3) 0.15)
                                                                 0.0)
                                   (cl-transforms:make-quaternion 0 0 0 1))))
             (retry))))
      (let* ((obj (perceive ?obj-name))
             (grasp (calculate-grasp obj))
             (pre-grasp-pos (calculate-pre-grasp obj))
             (grasp-vector (calculate-grasp-vector obj))
             (lift-vector (cl-transforms:make-3d-vector 0 0 0.1)))
        (ros-info (object-grasped cram-tutorial) "Opening gripper")
        (achieve `(gripper-open ,?side))
        (ros-info (object-grasped cram-tutorial) "Building collision map")
        (take-collision-map)
        (ros-info (object-grasped cram-tutorial) "Moving arm to pre-grasp")
        (with-failure-handling
            ((trajectory-controller-failed (e)
               (declare (ignore e))
               (retry)))
          (achieve `(arm-at absolute ,pre-grasp-pos ,grasp ,?side)))
        (ros-info (object-grasped cram-tutorial) "Grasping")
        (achieve `(arm-at relative ,grasp-vector ,?side))
        (achieve `(gripper-closed ,?side))
        (attach-bounding-box ?side obj)
        (attach-obj ?side obj)
        (ros-info (object-grasped cram-tutorial) "Lifting")
        (achieve `(arm-at relative ,lift-vector ,?side))))))

(def-goal (achieve (object-placed-at ?goal-pose ?side))
  (let ((pre-put-down-pose (calculate-pre-put-down-pose (get-obj ?side) ?goal-pose))
        (put-down-vector (calculate-put-down-vector (get-obj ?side))))
    (ros-info (object-grasped cram-tutorial) "Building collision map")
    (take-collision-map)
    (ros-info (object-placed cram-tutorial) "Moving arm to pre-put-down pose")
    (achieve `(arm-at absolute ,pre-put-down-pose ,(calculate-grasp (get-obj ?side)) ,?side))
    (ros-info (object-placed cram-tutorial) "Putting down")    
    (achieve `(arm-at relative ,put-down-vector ,?side))
    (achieve `(gripper-open ,?side))
    (detach-bounding-box ?side (get-obj ?side))
    (remove-obj ?side)))

(def-top-level-plan pick-and-place-exec-trace (obj side)
  (take-collision-map)
  (park-arm :left)
  (park-arm :right)
  (achieve `(robot-at ,(get-approach-pose (find-checkerboard))))
  (move-spine 0.3)
  (achieve `(robot-at ,(tf:make-pose-stamped
                        "/base_link" 0.0
                        (cl-transforms:make-3d-vector 0.5 0.0 0.0)
                        (cl-transforms:make-quaternion 0 0 0 1))))
  (achieve `(object-in-hand ,obj ,side))
  (robot-at (tf:make-pose-stamped
             "/base_link" 0.0
             (cl-transforms:make-3d-vector 0.0 -1.2 0.0)
             (cl-transforms:make-quaternion 0 0 0 1)))
  (let* ((obj (find-object "cluster_0"))
         (obj-pose (obj-pose obj)))
    (achieve `(object-placed-at ,(cl-tf:make-pose-stamped
                                  (tf:frame-id obj-pose)
                                  (tf:stamp obj-pose)
                                  (cl-transforms:v+
                                   (cl-transforms:origin obj-pose)
                                   (cl-transforms:make-3d-vector -0.1 -0.3 0))
                                  (cl-transforms:make-quaternion 0 0 0 1))
                                ,side))))
