修改后的carrot_planner

一个稍微修改了一些的carrot_planner, 用于搭配ftc_local_planner使用, 刚开始接触ROS的路径规划, 目前还相当不成熟, 希望大佬们可以提出宝贵意见!

修改如下

1. 原始global_plan中包含的点, 只有起点, 终点及其相应位姿, 现在做了插值, 并将这一连串的pose发布到/move_base_node/CarrotPlanner/my_path话题, 可以用于实时跟踪路径规划效果
2. ftc需要先转向目标点, 然后再移动, 当无法转向目标点时, 规划出倒车路径, 减小旋转导致的碰撞风险

直接下载官方包, ros-<ros-distro>-navigation包, 修改里面的carrot_planner, catkin_make以后就可以用了, 也可以修改名称, 重新打包为一个新的planner, 在应用时我修改的名称为 simple_global_planner
hpp:

#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 <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 Destructor
       */
      ~CarrotPlanner();

      /**
       * @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_;
      int segmentation;
      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
      ros::Publisher plan_pub_;
      /**
       * @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

cpp:

#include <angles/angles.h>
#include <carrot_planner/carrot_planner.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/PoseArray.h>
#include <algorithm>

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(carrot_planner::CarrotPlanner, nav_core::BaseGlobalPlanner)

namespace carrot_planner {

  CarrotPlanner::CarrotPlanner()
  : costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){}

  CarrotPlanner::CarrotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
  : costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){
    initialize(name, costmap_ros);
  }

  CarrotPlanner::~CarrotPlanner() {
    // deleting a nullptr is a noop
    delete world_model_;
  }
  
  void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
      costmap_ros_ = costmap_ros;
      costmap_ = costmap_ros_->getCostmap();

      ros::NodeHandle private_nh("~/" + name);
      private_nh.param("step_size", step_size_, costmap_->getResolution());
      private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
      plan_pub_ = private_nh.advertise<geometry_msgs::PoseArray>("my_path", 10);
      world_model_ = new base_local_planner::CostmapModel(*costmap_); 

      initialized_ = true;
    }
    else
      ROS_WARN("This planner has already been initialized... doing nothing");
  }

  //we need to take the footprint of the robot into account when we calculate cost to obstacles
  double CarrotPlanner::footprintCost(double x_i, double y_i, double theta_i){
    if(!initialized_){
      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
      return -1.0;
    }

    std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
    //if we have no footprint... do nothing
    if(footprint.size() < 3)
      return -1.0;

    //check if the footprint is legal
    double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);
    return footprint_cost;
  }


  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;
    }

    geometry_msgs::PoseArray path_msg;

    const double start_yaw = tf2::getYaw(start.pose.orientation);
    const double goal_yaw = tf2::getYaw(goal.pose.orientation);

    //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);
    path_msg.header.frame_id = "map";
    path_msg.poses.push_back(start.pose);
    
    // expand 2-points path to multi-points path to adjust teb/ftc planner
    double distance = std::sqrt( std::pow(start_x - target_x,2) + std::pow(start_y - target_y,2) );
    double interval_local_plan = 0.05;
    int count = distance / interval_local_plan;
    double delta_x = scale * diff_x / count;
    double delta_y = scale * diff_y / count;

    bool is_rotate_legal = true;
    // Define the range of angles for a full rotation (in radians)
    const double full_rotation_range = 2 * M_PI; // 360 degrees

    const double angle_increment = 0.1;

    // Check whether a collision will happen during rotating
    std::cout<<"==========================================================="<<std::endl;
    for (double angle = 0; angle < full_rotation_range; angle += angle_increment) 
    {
        is_rotate_legal = true;

        double rotate_footprint_cost = footprintCost(start.pose.position.x, start.pose.position.y,(start_yaw + angle));
        if(rotate_footprint_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
        {
            is_rotate_legal = false;
            std::cout<<"current rotate_footprint_cost: "<<rotate_footprint_cost<<std::endl;
            ROS_ERROR("Carrot: Rotate is illegal! Dropped!");
            std::cout<<"==========================================================="<<std::endl;
            break;
        } 
    }


    // If rotate is illegal, set rotation to smaller one(forward or backward)
    double requested_yaw = atan2((target_y-start_y),(target_x-start_x));
    double requested_rot_yaw = requested_yaw;
    if(!is_rotate_legal)   requested_rot_yaw = angles::normalize_angle(requested_yaw + M_PI);
    if(abs(requested_yaw - start_yaw)>abs(requested_rot_yaw - start_yaw)) requested_yaw = requested_rot_yaw;
    if(abs(requested_yaw - start_yaw)<abs(requested_rot_yaw - start_yaw)) requested_yaw = requested_yaw;

    is_rotate_legal = true;
    tf2::Quaternion requested_quat;
    requested_quat.setRPY(0,0,requested_yaw);

    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;
          local_goal.pose.orientation.x = requested_quat.x();
          local_goal.pose.orientation.y = requested_quat.y();
          local_goal.pose.orientation.z = requested_quat.z();
          local_goal.pose.orientation.w = requested_quat.w();
          plan.push_back(local_goal);
          path_msg.poses.push_back(local_goal.pose);
    }

    
    geometry_msgs::PoseStamped new_goal = goal;
    tf2::Quaternion goal_quat;
    goal_quat.setRPY(0, 0, 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);
    path_msg.poses.push_back(new_goal.pose);
    plan_pub_.publish(path_msg);


    return (done);
  }

};