text: /*********************************************************************
   *
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2009, 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: Eric Schneider
   *********************************************************************/
  #include <backup_recovery/backup_recovery.h>
  #include <pluginlib/class_list_macros.h>
  #include <unistd.h>
  
  //register this planner as a RecoveryBehavior plugin
  PLUGINLIB_DECLARE_CLASS(backup_recovery, BackupRecovery, backup_recovery::BackupRecovery, nav_core::RecoveryBehavior)
  
  namespace backup_recovery {
    BackupRecovery::BackupRecovery(): global_costmap_(NULL), local_costmap_(NULL), 
  				    tf_(NULL), initialized_(false), world_model_(NULL) {} 
  
    void BackupRecovery::initialize(std::string name, tf::TransformListener* tf,
  				  costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
      if(!initialized_){
        name_ = name;
        tf_ = tf;
        global_costmap_ = global_costmap;
        local_costmap_ = local_costmap;
  
        /*
        //get some parameters from the parameter server
        ros::NodeHandle private_nh("~/" + name_);
        ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
  
        //we'll simulate every degree by default
        private_nh.param("sim_granularity", sim_granularity_, 0.017);
        private_nh.param("frequency", frequency_, 20.0);
  
        blp_nh.param("acc_lim_th", acc_lim_th_, 3.2);
        blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0);
        blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4);
        blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10);
        */
  
        world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
  
        initialized_ = true;
      }
      else{
        ROS_ERROR("You should not call initialize twice on this object, doing nothing");
      }
    }
  
    BackupRecovery::~BackupRecovery(){
      delete world_model_;
    }
  
    void BackupRecovery::runBehavior(){
      if(!initialized_){
        ROS_ERROR("This object must be initialized before runBehavior is called");
        return;
      }
  
      if(global_costmap_ == NULL || local_costmap_ == NULL){
        ROS_ERROR("The costmaps passed to the BackupRecovery object cannot be NULL. Doing nothing.");
        return;
      }
      ROS_WARN("Backup recovery behavior started.");
  
      ros::Rate r(frequency_);
      ros::NodeHandle n;
      ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
  
      tf::Stamped<tf::Pose> global_pose;
      tf::Stamped<tf::Pose> local_pose;
  
      //global_costmap_->getRobotPose(global_pose);
      //local_costmap_->getRobotPose(local_pose);
  
      /*
      double current_angle = -1.0 * M_PI;
  
      bool got_180 = false;
  
      double start_offset = 0 - angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
      while(n.ok()){
        local_costmap_->getRobotPose(global_pose);
  
        double norm_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
        current_angle = angles::normalize_angle(norm_angle + start_offset);
  
        //compute the distance left to rotate
        double dist_left = M_PI - current_angle;
  
        double x = global_pose.getOrigin().x(), y = global_pose.getOrigin().y();
  
        //check if that velocity is legal by forward simulating
        double sim_angle = 0.0;
        while(sim_angle < dist_left){
  	double theta = tf::getYaw(global_pose.getRotation()) + sim_angle;
  
  	//make sure that the point is legal, if it isn't... we'll abort
  	double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
  	if(footprint_cost < 0.0){
  	  ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f", footprint_cost);
  	  return;
  	}
  
  	sim_angle += sim_granularity_;
        }
  
        //compute the velocity that will let us stop by the time we reach the goal
        double vel = sqrt(2 * acc_lim_th_ * dist_left);
  
        //make sure that this velocity falls within the specified limits
        vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
  
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0.0;
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = vel;
  
        vel_pub.publish(cmd_vel);
  
        //makes sure that we won't decide we're done right after we start
        if(current_angle < 0.0)
  	got_180 = true;
  
        //if we're done with our in-place rotation... then return
        if(got_180 && current_angle >= (0.0 - tolerance_))
  	return;
  
        r.sleep();
      }
      */
  
      for (int i=0; i<5; i++) {
        local_costmap_->getRobotPose(local_pose);
        global_costmap_->getRobotPose(global_pose);
  
        double local_x = local_pose.getOrigin().x() - 0.1;
        double local_y = local_pose.getOrigin().y();
        double local_theta = tf::getYaw(local_pose.getRotation());
  
        double global_x = global_pose.getOrigin().x();
        double global_y = global_pose.getOrigin().y();
        double global_theta = tf::getYaw(global_pose.getRotation());
  
        ROS_WARN("local_pose: (%.2f, %.2f) [%.3f]", local_x, local_y, local_theta);
        ROS_WARN("global_pose: (%.2f, %.2f) [%.3f]", global_x, global_y, global_theta);
  
        // Make sure that we don't back up into an obstacle
  
        double footprint_cost = world_model_->footprintCost(local_x, local_y, local_theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
  
        if ( footprint_cost < 0.0 ) {
  	  ROS_ERROR("Backup recovery can't back up because there is a potential collision. Cost: %.2f", footprint_cost);
  	  break;
        }
  
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = -0.2;
        cmd_vel.linear.y = 0.0;
      
        vel_pub.publish(cmd_vel);
  
        //r.sleep();
        usleep(200000);
      }
  
    } // end runBehavior()
  
  };
  