Ros 全局规划使用carrot_planner/CarrotPlanner和global_planner/GlobalPlanner

carrot_planner/CarrotPlanner的全局规划是使用简单的直线，不考虑代价地图，可以用于局部规划和定位精度测试
global_planner/GlobalPlanner的全局规划是使用A*算法，考虑代价地图，可以用于实际导航。
使用时在launch里简单替换就可以

		<!-- param name="base_global_planner" value="global_planner/GlobalPlanner" /-->
		<param name="base_global_planner" value="carrot_planner/CarrotPlanner" />

carrot_planner 修改如下

bool CarrotPlanner::makePlan(const geometry_msgs::PoseStamped& start, 
      const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
    
    

    if(!initialized_){
    
    
      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
      return false;
    }

    ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);

    plan.clear();
    costmap_ = costmap_ros_->getCostmap();

    if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
    
    
      ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.", 
          costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
      return false;
    }

    tf::Stamped<tf::Pose> goal_tf;
    tf::Stamped<tf::Pose> start_tf;

    poseStampedMsgToTF(goal,goal_tf);
    poseStampedMsgToTF(start,start_tf);

    double useless_pitch, useless_roll, goal_yaw, start_yaw;
    start_tf.getBasis().getEulerYPR(start_yaw, useless_pitch, useless_roll);
    goal_tf.getBasis().getEulerYPR(goal_yaw, useless_pitch, useless_roll);

    //we want to step back along the vector created by the robot's position and the goal pose until we find a legal cell
    double goal_x = goal.pose.position.x;
    double goal_y = goal.pose.position.y;
    double start_x = start.pose.position.x;
    double start_y = start.pose.position.y;

    double diff_x = goal_x - start_x;
    double diff_y = goal_y - start_y;
    double diff_yaw = angles::normalize_angle(goal_yaw-start_yaw);

    double target_x = goal_x;
    double target_y = goal_y;
    double target_yaw = goal_yaw;

    bool done = false;
    double scale = 1.0;
    double dScale = 0.01;

    while(!done)
    {
    
    
      if(scale < 0)
      {
    
    
        target_x = start_x;
        target_y = start_y;
        target_yaw = start_yaw;
        ROS_WARN("The carrot planner could not find a valid plan for this goal");
        break;
      }
      target_x = start_x + scale * diff_x;
      target_y = start_y + scale * diff_y;
      target_yaw = angles::normalize_angle(start_yaw + scale * diff_yaw);
      
      double footprint_cost = footprintCost(target_x, target_y, target_yaw);
      if(footprint_cost >= 0)
      {
    
    
          done = true;
      }
      scale -=dScale;
    }

    plan.push_back(start);

    //增加部分，目的是为了使用teb的寻迹功能
    double distance = std::sqrt( std::pow(start_x - target_x,2) + std::pow(start_y - target_y,2) );
    double interval_local_plan = 0.02;
    int count = distance / interval_local_plan;
    double delta_x = diff_x / count;
    double delta_y = diff_y / count;

    for(int i=0; i < count; i++)
    {
    
    
          geometry_msgs::PoseStamped local_goal = start;
          
          local_goal.pose.position.x = start_x + delta_x * i;
          local_goal.pose.position.y = start_y + delta_y * i;

          plan.push_back(local_goal);
    }

    geometry_msgs::PoseStamped new_goal = goal;
    tf::Quaternion goal_quat = tf::createQuaternionFromYaw(target_yaw);

    new_goal.pose.position.x = target_x;
    new_goal.pose.position.y = target_y;

    new_goal.pose.orientation.x = goal_quat.x();
    new_goal.pose.orientation.y = goal_quat.y();
    new_goal.pose.orientation.z = goal_quat.z();
    new_goal.pose.orientation.w = goal_quat.w();

    plan.push_back(new_goal);
    return (done);
  }