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

(define-condition no-objects-found (plan-error) ())

(defvar *collision-object-pub* nil)
(defvar *take-static-collision-map* nil)

(defun init-perception ()
  (setf *take-static-collision-map*
        (actionlib:make-action-client
         "/take_static_collision_map"
         "cotesys_ros_grasping/TakeStaticCollisionMapAction"))
  (setf *collision-object-pub*
        (advertise "/collision_object" "mapping_msgs/CollisionObject"))
  (actionlib:wait-for-server *take-static-collision-map*))

(register-ros-init-function init-perception)

(defun take-collision-map ()
  "Calls the /take_static_collision_map action"
  (call-action *take-static-collision-map*
                       (make-message "cotesys_ros_grasping/TakeStaticCollisionMapGoal")))

(defun find-objects ()
  "Returns all current object detections as a vector of CollisionObject messages."
  (publish *collision-object-pub*
           (make-msg "mapping_msgs/CollisionObject"
                     (frame_id header) "base_link"
                     (stamp header) (ros-time)
                     id "all"
                     (operation operation) 1))
  (with-fields (table_objects)
      (call-service "/detect_objects_srv/get_table_objects" "table_objects/GetCurrentObjects")
    (when (eql (length table_objects) 0)
      (fail 'no-objects-found))
    (map 'nil (lambda (co)
                (publish *collision-object-pub* co))
         table_objects)
    table_objects))

(defun find-checkerboard ()
  (with-fields (board_pose)
      (call-service "/get_checkerboard_pose"
                    "approach_table_tools/GetCheckerboardPose"
                    :corners_x 5 :corners_y 4
                    :spacing_x 0.08 :spacing_y 0.08)
    (msg->pose-stamped board_pose)))

(defun find-object (name)
  (find name (find-objects) :test #'equal :key #'obj-name))

(defun obj-name (obj)
  (mapping_msgs-msg:id-val obj))

(defun obj-pose (obj)
  (declare (type mapping_msgs-msg:<collisionobject> obj))
  (with-fields ((frame-id (frame_id header))
                poses)
        obj
      (with-fields ((x (x position))
                    (y (y position))
                    (z (z position))
                    (qx (x orientation))
                    (qy (y orientation))
                    (qz (z orientation))
                    (qw (w orientation)))
          (elt poses 0)
        (tf:make-pose-stamped
         (if (eql (elt frame-id 0) #\/)
             frame-id
             (concatenate 'string "/" frame-id))
         0.0
         (cl-transforms:make-3d-vector x y z)
         (cl-transforms:make-quaternion qx qy qz qw)))))

(defun obj-dimensions (obj)
  (with-fields (shapes) obj
    (with-fields (dimensions) (elt shapes 0)
      (cl-transforms:make-3d-vector
       (elt dimensions 0)
       (elt dimensions 1)
       (elt dimensions 2)))))
