修改后的carrot_planner

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

修改如下

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

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

#ifndef CARROT_PLANNER_H_
#define CARROT_PLANNER_H_
#include 
#include 
#include 
#include 

#include 

#include 
#include 

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

cpp:

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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

};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209