text: /*********************************************************************
  *
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2008, Willow Garage, Inc.
  *  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 Willow Garage, Inc. 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.
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
  #ifndef CARROT_PLANNER_H_
  #define CARROT_PLANNER_H_
  #include <ros/ros.h>
  #include <costmap_2d/costmap_2d_ros.h>
  #include <costmap_2d/costmap_2d.h>
  #include <nav_core/base_global_planner.h>
  
  #include <geometry_msgs/PoseStamped.h>
  #include <angles/angles.h>
  
  #include <base_local_planner/world_model.h>
  #include <base_local_planner/costmap_model.h>
  
  namespace carrot_planner{
    /**
     * @class CarrotPlanner
     * @brief Provides a simple global planner that will compute a valid goal point for the local planner by walking back along the vector between the robot and the user-specified goal point until a valid cost is found.
     */
    class CarrotPlanner : public nav_core::BaseGlobalPlanner {
      public:
        /**
         * @brief  Constructor for the CarrotPlanner
         */
        CarrotPlanner();
        /**
         * @brief  Constructor for the CarrotPlanner
         * @param  name The name of this planner
         * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
         */
        CarrotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  
        /**
         * @brief  Initialization function for the CarrotPlanner
         * @param  name The name of this planner
         * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
         */
        void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  
        /**
         * @brief Given a goal pose in the world, compute a plan
         * @param start The start pose 
         * @param goal The goal pose 
         * @param plan The plan... filled by the planner
         * @return True if a valid plan was found, false otherwise
         */
        bool makePlan(const geometry_msgs::PoseStamped& start, 
            const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
  
      private:
        costmap_2d::Costmap2DROS* costmap_ros_;
        double step_size_, min_dist_from_robot_;
        costmap_2d::Costmap2D* costmap_;
        base_local_planner::WorldModel* world_model_; ///< @brief The world model that the controller will use
  
        /**
         * @brief  Checks the legality of the robot footprint at a position and orientation using the world model
         * @param x_i The x position of the robot 
         * @param y_i The y position of the robot 
         * @param theta_i The orientation of the robot
         * @return 
         */
        double footprintCost(double x_i, double y_i, double theta_i);
  
        bool initialized_;
    };
  };  
  #endif
  