Navigating a robot using same path every time

edit

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 ...