sbpl_cart_planner.cpp

Go to the documentation of this file.

/*********************************************************************
*
* 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 the Willow Garage 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: Mike Phillips, Sachin Chitta
*********************************************************************/

#include <sbpl_cart_planner/sbpl_cart_planner.h>
#include <cart_pushing_msgs/RobotCartPath.h>
#include <pluginlib/class_list_macros.h>
#include <nav_msgs/Path.h>
#include <sbpl_cart_planner/SBPLCartPlannerStats.h>

using namespace std;
using namespace ros;

// TODO - uncomment code that forces environment to be empty!!!!

PLUGINLIB_REGISTER_CLASS(SBPLCartPlanner, SBPLCartPlanner, nav_core::BaseGlobalPlanner);

class LatticeSCQ : public StateChangeQuery{
  public:
    LatticeSCQ(EnvironmentNAVXYTHETACARTLAT* env, std::vector<nav2dcell_t> const & changedcellsV)
      : env_(env), changedcellsV_(changedcellsV) {
    }

    // lazy init, because we do not always end up calling this method
    virtual std::vector<int> const * getPredecessors() const{
      if(predsOfChangedCells_.empty() && !changedcellsV_.empty())
        env_->GetPredsofChangedEdges(&changedcellsV_, &predsOfChangedCells_);
      return &predsOfChangedCells_;
    }

    // lazy init, because we do not always end up calling this method
    virtual std::vector<int> const * getSuccessors() const{
      if(succsOfChangedCells_.empty() && !changedcellsV_.empty())
        env_->GetSuccsofChangedEdges(&changedcellsV_, &succsOfChangedCells_);
      return &succsOfChangedCells_;
    }

    EnvironmentNAVXYTHETACARTLAT * env_;
    std::vector<nav2dcell_t> const & changedcellsV_;
    mutable std::vector<int> predsOfChangedCells_;
    mutable std::vector<int> succsOfChangedCells_;
};

double SBPLCartPlanner::sign(double x)
{
  return x < 0.0 ? -1.0 : 1.0;
}

SBPLCartPlanner::SBPLCartPlanner()
  : initialized_(false), costmap_ros_(NULL){
}

SBPLCartPlanner::SBPLCartPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 
  : initialized_(false), costmap_ros_(NULL){
  initialize(name, costmap_ros);
  if(!initialized_)
    ROS_ERROR("Could not initialize global planner");
}

    
void SBPLCartPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
  if(!initialized_){
    num_sbpl_markers_ = 0;
    ros::NodeHandle private_nh("~/"+name);
    ros::NodeHandle nh(name);
    
    ROS_INFO("Name is %s", name.c_str());

    private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
    private_nh.param("allocated_time", allocated_time_, 10.0);
    private_nh.param("initial_epsilon",initial_epsilon_,3.0);
    private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
    private_nh.param("forward_search", forward_search_, bool(false));
    private_nh.param("primitive_filename",primitive_filename_,string(""));
    private_nh.param("force_scratch_limit",force_scratch_limit_,500);

    double nominalvel_mpersecs, timetoturn45degsinplace_secs;
    private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
    private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);

    private_nh.param<int>("visualizer_skip_poses",visualizer_skip_poses_,5);

    int lethal_obstacle;
    private_nh.param("lethal_obstacle",lethal_obstacle,20);
    lethal_obstacle_ = (unsigned char) lethal_obstacle;
    inscribed_inflated_obstacle_ = lethal_obstacle_-1;
    sbpl_cost_multiplier_ = (unsigned char) (costmap_2d::INSCRIBED_INFLATED_OBSTACLE/inscribed_inflated_obstacle_ + 1); 

    ROS_DEBUG("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz",lethal_obstacle,inscribed_inflated_obstacle_,sbpl_cost_multiplier_);
    costmap_ros_ = costmap_ros;
    costmap_ros_->clearRobotFootprint();


    costmap_ros_->getCostmapCopy(cost_map_);

    std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
    robot_footprint_ = footprint;
    if ("XYThetaLattice" == environment_type_){
      ROS_INFO("Using a 3D costmap for theta lattice\n");
      env_ = new EnvironmentNAVXYTHETACARTLAT();
    }
    else
    {
      ROS_FATAL("XYThetaLattice is currently the only supported environment!\n");
      return;
    }

    if(!env_->SetEnvParameter("cost_inscribed_thresh",inscribed_inflated_obstacle_))
    {
      ROS_FATAL("Failed to set cost_inscribed_thresh parameter");
      return;
    }
    //    env_->SetEnvParameter("cost_possibly_circumscribed_thresh", cost_map_.getCircumscribedCost());
    int obst_cost_thresh = costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE);
    vector<sbpl_2Dpt_t> perimeterptsV, cart_perimeterptsV;
    private_nh.param("cart_pivot_offset/x", cart_offset_.x, 0.35);
    private_nh.param("cart_pivot_offset/y", cart_offset_.y, 0.0);
    ROS_INFO("Cart pivot point offset (in base_footprint): %f %f",cart_offset_.x,cart_offset_.y);

    ros::NodeHandle cart_pushing_nh("cart_pushing");
    double cart_length, cart_width;
    double fp_x_offset, fp_y_offset;
    cart_pushing_nh.param("length", cart_length, 0.0);
    cart_pushing_nh.param("width", cart_width, 0.0);
    cart_pushing_nh.param("footprint_x_offset", fp_x_offset, 0.0);
    cart_pushing_nh.param("footprint_y_offset", fp_y_offset, 0.0);

    ROS_DEBUG("Length: %f %f, Footprint offset: %f %f",cart_length,cart_width,fp_x_offset,fp_y_offset);
    XmlRpc::XmlRpcValue init_position;
    geometry_msgs::Pose cart_init_pose;
    cart_pushing_nh.getParam("cart_init_pose/position", init_position);
    if(!(init_position.getType() == XmlRpc::XmlRpcValue::TypeArray && init_position.size() == 3)){
      ROS_FATAL("The init pose must be specified as a 3-Vector");
      return;
    }
    for(int i = 0; i < init_position.size(); ++i)
      if(!init_position[i].getType() == XmlRpc::XmlRpcValue::TypeDouble)//make sure that the value we're looking at a double
        ROS_FATAL("Values in the footprint specification must be numbers");
    cart_init_pose.position.x = (double) init_position[0];
    cart_init_pose.position.y = (double) init_position[1];
    cart_init_pose.position.z = (double) init_position[2];

    ROS_DEBUG("Init pose: %f %f %f",cart_init_pose.position.x,cart_init_pose.position.y,cart_init_pose.position.z);

    cart_cp_offset_.x = cart_init_pose.position.x - cart_offset_.x;
    cart_cp_offset_.y = cart_init_pose.position.y - cart_offset_.y;
    ROS_INFO("Cart control point offset (from pivot point): %f %f",cart_cp_offset_.x,cart_cp_offset_.y);

    
    double padding;
    std::string padding_param;
    if(!private_nh.searchParam("footprint_padding", padding_param))
      padding = 0.01;
    else
      private_nh.param(padding_param, padding, 0.01);
    

    std::vector<geometry_msgs::Point> cart_footprint;
    cart_footprint.resize(4);
    cart_footprint[0].x = -cart_length/2.0-padding;
    cart_footprint[0].y = -cart_width/2.0-padding;
    cart_footprint[0].z = 0.0;

    cart_footprint[1].x = -cart_length/2.0-padding;
    cart_footprint[1].y = cart_width/2.0+padding;
    cart_footprint[1].z = 0.0;

    cart_footprint[2].x = cart_length/2.0+padding;
    cart_footprint[2].y = cart_width/2.0+padding;
    cart_footprint[2].z = 0.0;

    cart_footprint[3].x = cart_length/2.0+padding;
    cart_footprint[3].y = -cart_width/2.0-padding;
    cart_footprint[3].z = 0.0;

    for(unsigned int i=0; i < cart_footprint.size(); i++)
      cart_footprint[i].x += (cart_init_pose.position.x + cart_length/2.0 - fp_x_offset -cart_offset_.x);


    //    std::vector<geometry_msgs::Point> cart_footprint = loadRobotFootprint(private_nh);
    cart_footprint_ = cart_footprint;
    cart_perimeterptsV.reserve(cart_footprint.size());
    ROS_INFO("Cart footprint");
    for (size_t ii(0); ii < cart_footprint.size(); ++ii) {
      sbpl_2Dpt_t pt;
      pt.x = cart_footprint[ii].x;
      pt.y = cart_footprint[ii].y;
      cart_perimeterptsV.push_back(pt);
      ROS_INFO("%d: %f %f",(int)ii,pt.x,pt.y);
    }
    perimeterptsV.reserve(footprint.size());
    for (size_t ii(0); ii < footprint.size(); ++ii) {
      sbpl_2Dpt_t pt;
      pt.x = footprint[ii].x;
      pt.y = footprint[ii].y;
      perimeterptsV.push_back(pt);
    }

    bool ret;
    try{
     ret = env_->InitializeEnv(costmap_ros_->getSizeInCellsX(), // width
                          costmap_ros_->getSizeInCellsY(), // height
                          0, // mapdata
                          0, 0, 0, 0, // start (x, y, theta, t)
                          0, 0, 0, 0, // goal (x, y, theta)
                          0, 0, 0, 0,//goal tolerance
                          perimeterptsV,
                          cart_offset_,
                          cart_perimeterptsV,
                          costmap_ros_->getResolution(), nominalvel_mpersecs,
                          timetoturn45degsinplace_secs, obst_cost_thresh,
                          primitive_filename_.c_str());
    }
    catch(SBPL_Exception e){
      ROS_ERROR("SBPL encountered a fatal exception");
      ret = false;
    }
    if(!ret){
      ROS_FATAL("SBPL initialization failed!");
    }

    for (ssize_t ix(0); ix < costmap_ros_->getSizeInCellsX(); ++ix)
      for (ssize_t iy(0); iy < costmap_ros_->getSizeInCellsY(); ++iy)
        env_->UpdateCost(ix, iy, costMapCostToSBPLCost(cost_map_.getCost(ix,iy)));
    
    if ("ARAPlanner" == planner_type_){
      planner_ = new ARAPlanner(env_, forward_search_);
    }
    else if ("ADPlanner" == planner_type_){
      planner_ = new ADPlanner(env_, forward_search_);
    }
    else{
      initialized_ = false;
      ROS_FATAL("ARAPlanner and ADPlanner are currently the only supported planners!\n");
      return;
    }

    ROS_INFO("[sbpl_cart_planner] Initialized successfully");
    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    sbpl_plan_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("sbpl_plan_array", 1);
    sbpl_plan_footprint_pub_ = private_nh.advertise<visualization_msgs::MarkerArray>("sbpl_plan_footprint_array", 1);
    sbpl_robot_cart_plan_pub_ = private_nh.advertise<cart_pushing_msgs::RobotCartPath>("sbpl_robot_cart_plan", 1);
    stats_publisher_ = private_nh.advertise<sbpl_cart_planner::SBPLCartPlannerStats>("sbpl_cart_planner_stats", 1);
    
    if(!env_->initialized_)
      initialized_ = false;
    else
      initialized_ = true;
  }

}

unsigned char SBPLCartPlanner::costMapCostToSBPLCost(unsigned char newcost)
{
  if(newcost == costmap_2d::LETHAL_OBSTACLE)
    return lethal_obstacle_;
  else if(newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
    return inscribed_inflated_obstacle_;
  else if(newcost == 0 || newcost == costmap_2d::NO_INFORMATION)
    return 0;
  else
    return (unsigned char) (newcost/sbpl_cost_multiplier_ + 0.5);

}

std::vector<geometry_msgs::Point> SBPLCartPlanner::loadRobotFootprint(ros::NodeHandle node)
{
  std::vector<geometry_msgs::Point> footprint;
  geometry_msgs::Point pt;
  double padding;
  
  std::string padding_param, footprint_param;
  if(!node.searchParam("footprint_padding", padding_param))
    padding = 0.01;
  else
    node.param(padding_param, padding, 0.01);
  
  //grab the footprint from the parameter server if possible
  XmlRpc::XmlRpcValue footprint_list;
  if(node.searchParam("cart_footprint", footprint_param)){
    ROS_INFO("Footprint param: %s", footprint_param.c_str());
    node.getParam(footprint_param, footprint_list);
    //make sure we have a list of lists
    if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2)){
      ROS_FATAL("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
      throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
    }
    for(int i = 0; i < footprint_list.size(); ++i){
      //make sure we have a list of lists of size 2
      XmlRpc::XmlRpcValue point = footprint_list[i];
      if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
        ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
        throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
      }
      
      //make sure that the value we're looking at is either a double or an int
      if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
        ROS_FATAL("Values in the footprint specification must be numbers");
        throw std::runtime_error("Values in the footprint specification must be numbers");
      }
      pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
      pt.x += sign(pt.x) * padding;
      
      //make sure that the value we're looking at is either a double or an int
      if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
        ROS_FATAL("Values in the footprint specification must be numbers");
        throw std::runtime_error("Values in the footprint specification must be numbers");
      }
      pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
      pt.y += sign(pt.y) * padding;
      
      footprint.push_back(pt);
      
    }
  }
    return footprint;
}

  
bool SBPLCartPlanner::makePlan(const geometry_msgs::PoseStamped& start,
                               const geometry_msgs::PoseStamped& goal,
                               std::vector<geometry_msgs::PoseStamped>& plan){
  if(!initialized_)
  {
    ROS_ERROR("Global planner is not initialized");
    return false;
  }

  plan.clear();

  ROS_INFO("[sbpl_lattice_planner] getting fresh copy of costmap");
  costmap_ros_->clearRobotFootprint();
  ROS_INFO("[sbpl_lattice_planner] robot footprint cleared");
  geometry_msgs::Pose cart_pose = getGlobalCartPose(start.pose,0.0);
  costmap_ros_->getCostmapCopy(cost_map_);
  clearFootprint(cart_pose,cart_footprint_);

  ROS_INFO("[sbpl_lattice_planner] getting start point (%g,%g) goal point (%g,%g)",
           start.pose.position.x, start.pose.position.y,goal.pose.position.x, goal.pose.position.y);
  double theta_start = 2 * atan2(start.pose.orientation.z, start.pose.orientation.w);
  double theta_goal = 2 * atan2(goal.pose.orientation.z, goal.pose.orientation.w);
  double cart_start = 0.0;
  double cart_goal = 0.0;

  try
  {
    int ret = env_->SetStart(start.pose.position.x - cost_map_.getOriginX(), start.pose.position.y - cost_map_.getOriginY(), theta_start, cart_start);
    if(ret < 0 || planner_->set_start(ret) == 0){
      ROS_ERROR("Failed to set start state");
      return false;
    }    
  }
  catch(SBPL_Exception e){
    ROS_ERROR("SBPL encountered a fatal exception while setting the start state");
    return false;
  }

  try{
    int ret = env_->SetGoal(goal.pose.position.x - cost_map_.getOriginX(), goal.pose.position.y - cost_map_.getOriginY(), theta_goal, cart_goal);
    if(ret < 0 || planner_->set_goal(ret) == 0){
      ROS_ERROR("Failed to set goal state");
      return false;
    }
  }
  catch(SBPL_Exception e){
    ROS_ERROR("SBPL encountered a fatal exception while setting the goal state");
    return false;
  }

  
  int offOnCount = 0;
  int onOffCount = 0;
  int allCount = 0;
  vector<nav2dcell_t> changedcellsV;

  for(unsigned int ix = 0; ix < cost_map_.getSizeInCellsX(); ix++) {
    for(unsigned int iy = 0; iy < cost_map_.getSizeInCellsY(); iy++) {

      unsigned char oldCost = env_->GetMapCost(ix,iy);
      unsigned char newCost = cost_map_.getCost(ix,iy);

      if(oldCost == newCost) continue;

      allCount++;

      //first case - off cell goes on

      if((oldCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && oldCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
         (newCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || newCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
        offOnCount++;
      }
      
      if((oldCost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || oldCost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) &&
         (newCost != costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) && newCost != costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))) {
        onOffCount++;
      }      
      env_->UpdateCost(ix, iy, costMapCostToSBPLCost(cost_map_.getCost(ix,iy)));

      nav2dcell_t nav2dcell;
      nav2dcell.x = ix;
      nav2dcell.y = iy;
      changedcellsV.push_back(nav2dcell);
    }
  }

  try{   
    if(!changedcellsV.empty()){
      StateChangeQuery* scq = new LatticeSCQ(env_, changedcellsV);
      planner_->costs_changed(*scq);
      delete scq;
    }    
    if(allCount > force_scratch_limit_)
      planner_->force_planning_from_scratch();
  }
  catch(SBPL_Exception e){
    ROS_ERROR("SBPL failed to update the costmap");
    return false;
  }

  //setting planner parameters
  ROS_INFO("allocated:%f, init eps:%f\n",allocated_time_,initial_epsilon_);
  planner_->set_initialsolution_eps(initial_epsilon_);
  planner_->set_search_mode(false);

  ROS_INFO("[sbpl_lattice_planner] run planner");
  vector<int> solution_stateIDs;
  int solution_cost;
  try{
    int ret = planner_->replan(allocated_time_, &solution_stateIDs, &solution_cost);
    if(ret)
      ROS_DEBUG("Solution is found\n");
    else
    {
      ROS_INFO("Solution does not exist\n");    
      ARAPlanner *ara_planner = dynamic_cast<ARAPlanner*>(planner_);
      // Fill up statistics and publish
      sbpl_cart_planner::SBPLCartPlannerStats stats;
      stats.initial_epsilon = initial_epsilon_;
      stats.plan_to_first_solution = false;
      stats.allocated_time = allocated_time_;
      if(ara_planner)
      {
        stats.actual_time = ara_planner->get_final_eps_planning_time();    
        stats.actual_time = ara_planner->get_initial_eps_planning_time();
        stats.number_of_expands_initial_solution = ara_planner->get_n_expands_init_solution();
        stats.final_epsilon = ara_planner->get_final_epsilon();
      }
      stats.solution_cost = solution_cost;
      stats.path_size = 0;
      stats.start = start;
      stats.goal = goal;
      stats.start_cart_angle = cart_start;
      stats.goal_cart_angle = cart_goal;
      stats_publisher_.publish(stats);
      stats.final_number_of_expands = planner_->get_n_expands();
      return false;
    }
  }
  catch(SBPL_Exception e){
    ROS_ERROR("SBPL encountered a fatal exception while planning");
    return false;
  }

  ROS_DEBUG("size of solution=%d", (int)solution_stateIDs.size());

  vector<EnvNAVXYTHETACARTLAT3Dpt_t> sbpl_path;
  try
  {
    env_->ConvertStateIDPathintoXYThetaPath(&solution_stateIDs, &sbpl_path);
  }
  catch(SBPL_Exception e)
  {
    ROS_ERROR("SBPL encountered a fatal exception while reconstructing the path");
    return false;
  }

  ROS_INFO("Plan has %d points.\n", (int)sbpl_path.size());
  ros::Time plan_time = ros::Time::now();

  //
  visualization_msgs::MarkerArray sbpl_plan_marker_array;
  convertPathToMarkerArray(sbpl_path,costmap_ros_->getGlobalFrameID(),sbpl_plan_marker_array);
  sbpl_plan_pub_.publish(sbpl_plan_marker_array);

  visualization_msgs::MarkerArray sbpl_plan_footprint;
  getFootprintList(sbpl_path,costmap_ros_->getGlobalFrameID(),sbpl_plan_footprint);
  sbpl_plan_footprint_pub_.publish(sbpl_plan_footprint);

  //create a message for the plan 
  nav_msgs::Path gui_path;
  gui_path.poses.resize(sbpl_path.size());
  gui_path.header.frame_id = costmap_ros_->getGlobalFrameID();
  gui_path.header.stamp = plan_time;

  cart_pushing_msgs::RobotCartPath sbpl_robot_cart_plan;
  sbpl_robot_cart_plan.header.frame_id = costmap_ros_->getGlobalFrameID();
  sbpl_robot_cart_plan.header.stamp = plan_time;


  for(unsigned int i=0; i<sbpl_path.size(); i++){
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = plan_time;
    pose.header.frame_id = costmap_ros_->getGlobalFrameID();

    pose.pose.position.x = sbpl_path[i].x + cost_map_.getOriginX();
    pose.pose.position.y = sbpl_path[i].y + cost_map_.getOriginY();
    pose.pose.position.z = start.pose.position.z;

    btQuaternion temp;
    temp.setEulerZYX(sbpl_path[i].theta,0,0);
    pose.pose.orientation.x = temp.getX();
    pose.pose.orientation.y = temp.getY();
    pose.pose.orientation.z = temp.getZ();
    pose.pose.orientation.w = temp.getW();

    plan.push_back(pose);

    cart_pushing_msgs::RobotCartConfiguration robot_cart_configuration;
    robot_cart_configuration.robot_pose = pose.pose;
    robot_cart_configuration.cart_pose = getLocalCartControlFramePose(sbpl_path[i]);
    ROS_DEBUG("Cartangle: %d %f",i,sbpl_path[i].cartangle);
    sbpl_robot_cart_plan.path.push_back(robot_cart_configuration);

    gui_path.poses[i].pose.position.x = plan[i].pose.position.x;
    gui_path.poses[i].pose.position.y = plan[i].pose.position.y;
    gui_path.poses[i].pose.position.z = plan[i].pose.position.z;
  }
  plan_pub_.publish(gui_path);
  sbpl_robot_cart_plan_pub_.publish(sbpl_robot_cart_plan);

  ARAPlanner *ara_planner = dynamic_cast<ARAPlanner*>(planner_);

  // Fill up statistics and publish
  sbpl_cart_planner::SBPLCartPlannerStats stats;
  stats.initial_epsilon = initial_epsilon_;
  stats.plan_to_first_solution = false;
  stats.final_number_of_expands = planner_->get_n_expands();
  stats.allocated_time = allocated_time_;
  if(ara_planner)
  {
    stats.time_to_first_solution = ara_planner->get_initial_eps_planning_time();
    stats.actual_time = ara_planner->get_final_eps_planning_time();    
    stats.number_of_expands_initial_solution = ara_planner->get_n_expands_init_solution();
    stats.final_epsilon = ara_planner->get_final_epsilon();
  }
  stats.solution_cost = solution_cost;
  stats.path_size = sbpl_path.size();
  stats.start = start;
  stats.goal = goal;
  stats.start_cart_angle = cart_start;
  stats.goal_cart_angle = cart_goal;
  stats.solution = sbpl_robot_cart_plan;
  stats_publisher_.publish(stats);

  return true;
}

void SBPLCartPlanner::convertPathToMarkerArray(const std::vector<EnvNAVXYTHETACARTLAT3Dpt_t> &sbpl_path,
                                               const std::string &path_frame_id,
                                               visualization_msgs::MarkerArray &ma)
{
  unsigned int num_markers = sbpl_path.size();
  ma.markers.resize(2*num_markers);

  for(unsigned int i=0; i< num_markers; i++)
  {
    ma.markers[2*i].header.frame_id = path_frame_id;
    ma.markers[2*i].header.stamp = ros::Time();
    ma.markers[2*i].ns = "sbpl_cart_plan";
    ma.markers[2*i].id = 2*i;
    ma.markers[2*i].type = visualization_msgs::Marker::ARROW;
    ma.markers[2*i].action = visualization_msgs::Marker::ADD;
    ma.markers[2*i].pose.position.x = sbpl_path[i].x;
    ma.markers[2*i].pose.position.y = sbpl_path[i].y;
    ma.markers[2*i].pose.position.z = 0.0;
    btQuaternion quat;
    quat.setRPY(0.0,0.0,sbpl_path[i].theta);
    tf::quaternionTFToMsg(quat,ma.markers[2*i].pose.orientation);
    ma.markers[2*i].scale.x = 0.1;
    ma.markers[2*i].scale.y = 0.05;
    ma.markers[2*i].scale.z = 0.05;
    ma.markers[2*i].color.a = 1.0;
    ma.markers[2*i].color.r = 0.0;
    ma.markers[2*i].color.g = 1.0;
    ma.markers[2*i].color.b = 0.0;

    geometry_msgs::Pose cart_pose;
    cart_pose.position.x = sbpl_path[i].x + cos(sbpl_path[i].theta)*cart_offset_.x - sin(sbpl_path[i].theta)*cart_offset_.y;
    cart_pose.position.y = sbpl_path[i].y + sin(sbpl_path[i].theta)*cart_offset_.x + cos(sbpl_path[i].theta)*cart_offset_.y;
    cart_pose.position.z = 0.0;
    double yaw = sbpl_path[i].theta + sbpl_path[i].cartangle;
    quat.setRPY(0.0,0.0,yaw);
    tf::quaternionTFToMsg(quat,cart_pose.orientation);

    ma.markers[2*i+1].header.frame_id = path_frame_id;
    ma.markers[2*i+1].header.stamp = ros::Time();
    ma.markers[2*i+1].ns = "sbpl_cart_plan";
    ma.markers[2*i+1].id = 2*i+1;
    ma.markers[2*i+1].type = visualization_msgs::Marker::ARROW;
    ma.markers[2*i+1].action = visualization_msgs::Marker::ADD;
    ma.markers[2*i+1].pose = cart_pose;
    ma.markers[2*i+1].scale.x = 0.1;
    ma.markers[2*i+1].scale.y = 0.05;
    ma.markers[2*i+1].scale.z = 0.05;
    ma.markers[2*i+1].color.a = 1.0;
    ma.markers[2*i+1].color.r = 1.0;
    ma.markers[2*i+1].color.g = 0.0;
    ma.markers[2*i+1].color.b = 0.0;
  }
  if(num_markers < num_sbpl_markers_)
  {
    for(unsigned int i = 0; i < (num_sbpl_markers_ - num_markers); i++)
    {     
      visualization_msgs::Marker new_marker;
      new_marker.header.frame_id = path_frame_id;
      new_marker.header.stamp = ros::Time();
      new_marker.ns = "sbpl_cart_plan";
      new_marker.id = 2*(num_markers+i);
      new_marker.type = visualization_msgs::Marker::ARROW;
      new_marker.action = visualization_msgs::Marker::DELETE;

      ma.markers.push_back(new_marker);

      new_marker.id = 2*(num_markers+i)+1;

      ma.markers.push_back(new_marker);

    }
  }
  num_sbpl_markers_ = num_markers;
}



geometry_msgs::Pose SBPLCartPlanner::getGlobalCartPose(const EnvNAVXYTHETACARTLAT3Dpt_t& sbpl_pose)
{
  geometry_msgs::Pose cart_pose;
  btQuaternion quat;
  cart_pose.position.x = sbpl_pose.x + cos(sbpl_pose.theta)*cart_offset_.x - sin(sbpl_pose.theta)*cart_offset_.y;
  cart_pose.position.y = sbpl_pose.y + sin(sbpl_pose.theta)*cart_offset_.x + cos(sbpl_pose.theta)*cart_offset_.y;
  cart_pose.position.z = 0.0;
  double yaw = sbpl_pose.theta + sbpl_pose.cartangle;
  quat.setRPY(0.0,0.0,yaw);
  tf::quaternionTFToMsg(quat,cart_pose.orientation);
  return cart_pose;
}


geometry_msgs::Pose SBPLCartPlanner::getLocalCartControlFramePose(const EnvNAVXYTHETACARTLAT3Dpt_t& sbpl_pose)
{
  geometry_msgs::Pose cart_pose;
  btQuaternion quat;
  cart_pose.position.x = cart_offset_.x + cos(sbpl_pose.cartangle)*cart_cp_offset_.x - sin(sbpl_pose.cartangle)*cart_cp_offset_.y;
  cart_pose.position.y = cart_offset_.y + sin(sbpl_pose.cartangle)*cart_cp_offset_.x + cos(sbpl_pose.cartangle)*cart_cp_offset_.y;
  cart_pose.position.z = 0.0;
  double yaw = sbpl_pose.cartangle;
  quat.setRPY(0.0,0.0,yaw);
  tf::quaternionTFToMsg(quat,cart_pose.orientation);
  return cart_pose;
}

geometry_msgs::Pose SBPLCartPlanner::getLocalCartPose(const EnvNAVXYTHETACARTLAT3Dpt_t& sbpl_pose)
{
  geometry_msgs::Pose cart_pose;
  btQuaternion quat;
  cart_pose.position.x = cart_offset_.x;
  cart_pose.position.y = cart_offset_.y;
  cart_pose.position.z = 0.0;
  double yaw = sbpl_pose.cartangle;
  quat.setRPY(0.0,0.0,yaw);
  tf::quaternionTFToMsg(quat,cart_pose.orientation);
  return cart_pose;
}

void SBPLCartPlanner::getFootprintList(const std::vector<EnvNAVXYTHETACARTLAT3Dpt_t> &sbpl_path,
                                       const std::string &path_frame_id,
                                       visualization_msgs::MarkerArray & ma)
{ 
  ma.markers.resize(2); 
  ma.markers[0].header.frame_id = path_frame_id;
  ma.markers[0].header.stamp = ros::Time();
  ma.markers[0].ns = "sbpl_cart_footprint";
  ma.markers[0].id = 0;
  ma.markers[0].type = visualization_msgs::Marker::LINE_LIST;
  ma.markers[0].action = visualization_msgs::Marker::ADD;

  ma.markers[0].scale.x = 0.05;
  ma.markers[0].color.a = 1.0;
  ma.markers[0].color.r = 0.0;
  ma.markers[0].color.g = 0.0;
  ma.markers[0].color.b = 1.0;

  ma.markers[1].header.frame_id = path_frame_id;
  ma.markers[1].header.stamp = ros::Time();
  ma.markers[1].ns = "sbpl_cart_footprint";
  ma.markers[1].id = 1;
  ma.markers[1].type = visualization_msgs::Marker::LINE_LIST;
  ma.markers[1].action = visualization_msgs::Marker::ADD;

  ma.markers[1].scale.x = 0.05;
  ma.markers[1].color.a = 1.0;
  ma.markers[1].color.r = 0.0;
  ma.markers[1].color.g = 1.0;
  ma.markers[1].color.b = 0.0;


  for(unsigned int i=0; i <  sbpl_path.size(); i= i+visualizer_skip_poses_)
  {
    std::vector<geometry_msgs::Point> transformed_cfp, transformed_rfp;
    geometry_msgs::Pose robot_pose;
    robot_pose.position.x = sbpl_path[i].x;
    robot_pose.position.y = sbpl_path[i].y;
    robot_pose.position.z = 0.0;
    btQuaternion quat;
    quat.setRPY(0.0,0.0,sbpl_path[i].theta);
    tf::quaternionTFToMsg(quat,robot_pose.orientation);

    geometry_msgs::Pose cart_pose;
    cart_pose.position.x = sbpl_path[i].x + cos(sbpl_path[i].theta)*cart_offset_.x - sin(sbpl_path[i].theta)*cart_offset_.y;
    cart_pose.position.y = sbpl_path[i].y + sin(sbpl_path[i].theta)*cart_offset_.x + cos(sbpl_path[i].theta)*cart_offset_.y;
    cart_pose.position.z = 0.0;
    double yaw = sbpl_path[i].theta + sbpl_path[i].cartangle;
    quat.setRPY(0.0,0.0,yaw);
    tf::quaternionTFToMsg(quat,cart_pose.orientation);

    transformFootprintToEdges(robot_pose,robot_footprint_,transformed_rfp);
    transformFootprintToEdges(cart_pose,cart_footprint_,transformed_cfp);

    for(unsigned int i=0; i < transformed_rfp.size(); i++)
      ma.markers[0].points.push_back(transformed_rfp[i]);

    for(unsigned int i=0; i < transformed_cfp.size(); i++)
      ma.markers[1].points.push_back(transformed_cfp[i]);
  }

  return; 
}

void SBPLCartPlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose,
                                         const std::vector<geometry_msgs::Point> &footprint,
                                         std::vector<geometry_msgs::Point> &out_footprint)
{
  out_footprint.resize(2*footprint.size());
  double yaw = tf::getYaw(robot_pose.orientation);
  for(unsigned int i=0; i < footprint.size(); i++)
  {
    out_footprint[2*i].x = robot_pose.position.x + cos(yaw)*footprint[i].x - sin(yaw)*footprint[i].y;
    out_footprint[2*i].y = robot_pose.position.y + sin(yaw)*footprint[i].x + cos(yaw)*footprint[i].y;
    if(i == 0)
    {
      out_footprint.back().x = out_footprint[i].x;
      out_footprint.back().y = out_footprint[i].y;
    }
    else
    {
      out_footprint[2*i-1].x = out_footprint[2*i].x;
      out_footprint[2*i-1].y = out_footprint[2*i].y;
    }
  }
  return;
}

bool SBPLCartPlanner::clearFootprint(const geometry_msgs::Pose &robot_pose, 
                                     const std::vector<geometry_msgs::Point> &footprint)
{  
  vector<geometry_msgs::Point> oriented_footprint;
  getOrientedFootprint(robot_pose, footprint, oriented_footprint);
  
  //we also want to clear the robot footprint from the costmap we're using
  if(!cost_map_.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE)) 
  {
      ROS_ERROR("Could not clear robot footprint: ");
      for (uint i = 0; i < oriented_footprint.size(); ++i)
        ROS_ERROR("oriented_footprint: %.2lf %.2lf %.2lf", oriented_footprint[i].x, oriented_footprint[i].y, oriented_footprint[i].z);
      return false;     
  }
  return true;
}

void SBPLCartPlanner::getOrientedFootprint(const geometry_msgs::Pose &robot_pose,
                                           const std::vector<geometry_msgs::Point> &footprint,
                                           std::vector<geometry_msgs::Point> &oriented_footprint)
{
  oriented_footprint.resize(footprint.size());
  double yaw = tf::getYaw(robot_pose.orientation);
  for(unsigned int i=0; i < footprint.size(); i++)
  {
    oriented_footprint[i].x = robot_pose.position.x + cos(yaw)*footprint[i].x - sin(yaw)*footprint[i].y;
    oriented_footprint[i].y = robot_pose.position.y + sin(yaw)*footprint[i].x + cos(yaw)*footprint[i].y;
  }
  return;
}

geometry_msgs::Pose SBPLCartPlanner::getGlobalCartPose(const geometry_msgs::Pose &robot_pose, const double &cart_angle)
{
  EnvNAVXYTHETACARTLAT3Dpt_t sbpl_pose;
  sbpl_pose.x = robot_pose.position.x;
  sbpl_pose.y = robot_pose.position.y;
  sbpl_pose.theta = tf::getYaw(robot_pose.orientation);
  sbpl_pose.cartangle = cart_angle;
  return getGlobalCartPose(sbpl_pose);
}