Revision history [back]

I've actually wrote a planner that does something similar, so I'll post the code for the initialize and makePlan methods for you, or anyone that is interested. It's nothing too sophisticated, but it works for just following a loop. By following the tutorial for writing a global planner you should be able to fill in the header file and create the plugin.

The initialize method loads a tab delineated text file with X and Y coordinates for the path you want to follow, and stores this as the path.

The parameters that get loaded are for setting the filename of the text file with coordinates, the package it resides in, and also how far apart points are spaced in your text file, and how far ahead you want to generate the plan. If planning_distance_ is negative, then it will generate a plan for the entire loop (minus 0.5m, otherwise move_base will think its reached its goal).

    void LoopPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
      costmap_ros_ = costmap_ros;

      ros::NodeHandle private_nh("~/" + name);

      // Get parameters
      private_nh.param("pt_spacing", pt_spacing_, 0.01);
      private_nh.param("planning_distance", planning_distance_, 2.0);
      if (!private_nh.getParam("path_filename", path_filename_)){
        ROS_ERROR("Failed to get param 'path_filename'. A text file with coordinates is necessary for this planner to run.");
      }// end if
      if (!private_nh.getParam("path_package", path_package_)){
        ROS_ERROR("Failed to get param 'path_package'. This is necessary in order to find the path file.");
      }// end if

      // Set inputted file name and path
      std::string full_file = ros::package::getPath(path_package_) + "/" + path_filename_;

      // Load file
      std::ifstream is(full_file.c_str());
      std::istream_iterator<double> start(is), end;
      std::vector<double> data(start, end);

      // Resize vectors appropriately
      int columns = 2;
      num_pts_ = data.size()/columns;
      path_x_.resize(num_pts_);
      path_y_.resize(num_pts_);

      // Sort data columns into vectors
      for (int i =0 ; i < num_pts_ ; i++){
        path_x_[i] = data[columns*i];
        path_y_[i] = data.at(columns*i+1);
       }// end for

      initialized_ = true;

      ROS_INFO("Created global_planner loop_planner");
      ROS_INFO("Loaded path file");
    }
    else
      ROS_WARN("This planner has already been initialized... doing nothing");
  }// end initialize

Then in the makePlan method it finds the closest point on the path to the starting point, then builds the plan from there. It doesn't check if any of the points are valid in the costmap, since for my use I always knew they were valid.

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

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

    // Make sure the plan vector is empty
    plan.clear();

    // Check if goal and costmap frame ID's match
    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;
    }// end if

    ROS_DEBUG("Starting path generation");
    ROS_DEBUG("Finding closest point on path");

    // Create vectors to hold the difference to each point
    std::vector<double> x_diff (num_pts_);
    std::vector<double> y_diff (num_pts_);

    // Initialize the current minimum distance to the first path point
    double dist_x = (start.pose.position.x - path_x_[0]);
    double dist_y = (start.pose.position.x - path_y_[0]);
    double dist_min = sqrt(pow(dist_x, 2) + pow(dist_y, 2));

    // For storing what the minimum distance is, and where it is
    double dist;
    int index = 0;

    // Check each point
    for(int i = 0 ; i < num_pts_; i++){
      dist_x = (start.pose.position.x - path_x_[i]);
      dist_y = (start.pose.position.y - path_y_[i]);
      dist = sqrt(pow(dist_x, 2) + pow(dist_y, 2));

      if (dist < dist_min){
        dist_min = dist;
        index = i;
      }// end if
    }// end for

    ROS_DEBUG("Start point #%d, X=%.3f, Y=%.3f", index + 1, path_x_[index], path_y_[index]);

    // Creat temp goal to feed into plan (equal to goal because we need the tf info)
    geometry_msgs::PoseStamped temp_goal = goal;

    // How many points we want to look ahead
    int pts_ahead;
    if (planning_distance_ < 0) {
      // Loop to within 0.5m of start point, so it doesn't think we've reached our goal
      pts_ahead = num_pts_ - 0.5 / pt_spacing_;
    }else {
      pts_ahead = planning_distance_ / pt_spacing_;
    }// end if

    // Assign all the goal points
    for (int i = 0 ; i < pts_ahead;  i++) {

      // Increment, and loop back to zero if we've surpassed num_pts
      index++;
      index = index % num_pts_;

      // Fill out temp goal
      temp_goal.pose.position.x = path_x_[index];
      temp_goal.pose.position.y = path_y_[index];

      // Must assign a proper orientation for the last point
      if (i == (pts_ahead - 1)){
        double x_component = path_x_[(index + 1) % num_pts_] - path_x_[index];
        double y_component = path_y_[(index + 1) % num_pts_] - path_y_[index];
        double yaw = atan2(y_component, x_component);

        tf::Quaternion temp_goal_quat = tf::createQuaternionFromYaw(yaw);
        temp_goal.pose.orientation.x = temp_goal_quat.x();
        temp_goal.pose.orientation.y = temp_goal_quat.y();
        temp_goal.pose.orientation.z = temp_goal_quat.z();
        temp_goal.pose.orientation.w = temp_goal_quat.w();   
      }// end if

      plan.push_back(temp_goal);
    }// end for

    ROS_DEBUG("End point #%d, X=%.3f, Y=%.3f", index + 1, path_x_[index], path_y_[index]);

    return (true);

  }// end makePlan