;;;
;;; 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 *move-base-client* nil)

(defun init-navigation ()
  "Initialized the move_base client"
  (setf *move-base-client*
        (actionlib:make-action-client
         "/nav_pcontroller/move_base"
         "move_base_msgs/MoveBaseAction"))
  (actionlib:wait-for-server *move-base-client*))
(register-ros-init-function init-navigation)

(defun nav-to (pose)
  "Navigates the robot to `pose' by calling the move_base action.

`pose' is a cl-tf:pose-stamped, representing the goal pose of the action"
  (declare (type cl-tf:pose-stamped pose))
  (actionlib:call-goal
   *move-base-client*
   (make-message "move_base_msgs/MoveBaseGoal"
                 target_pose (pose-stamped->msg pose))))

(defun get-approach-pose (board-pose)
  (with-fields (nav_pose)
      (call-service "/get_approach_pose"
                    "approach_table_tools/GetApproachPose"
                    :board_pose (pose-stamped->msg board-pose))
    (msg->pose-stamped nav_pose)))
