Rosanswers logo


I am not sure if I am clear here with my question. What I want to do is create a map with couple of obstacles in it and to make the robot drive around them, making an 8 shape. Something similar to the picture. Just imagine that in those 2 loops, there would be an obstacle. I am probably using wrong terms to find a similar example. I want the robot to be always driving that shape too (this is necessary for tests). Which tutorials could provide me with some samples? Right now I am using teb_local_planner. One of the requirements for the robot is that it also has to localize itself while doing this.

image description http://png.clipart.me/graphics/thumbs/139/background-with-road-in-the-form-of-eight-vector-illustration_139624610.jpg

Thank you!

EDIT So I have tried running samples of teb_local_planner following the Global Plan (Via-Points), however it didnt work for me.

I couldnt git clone the newest version of teb_local_planner because I already had it in my ~/catkin_ws directory (or at least I got git FATAL error), however I ran the rosdep install in the same folder and it didnt complain. How do I know that it didnt work in the end? I could never see neither the global_plan_viapoint_sep parameter nor weight_viapoint when I ran the roslaunch teb_local_planner test_optim_node.launch. Heres proof:

https://www.dropbox.com/s/axq237aul1v1huu/Screenshot%202016-05-09%2020.11.06.png?dl=0 https://www.dropbox.com/s/l6x1a4cxgkg6wb7/Screenshot%202016-05-09%2020.11.01.png?dl=0

I tried setting it just in rqt config file, but to no avail.

Originally posted by murdock on ROS Answers with karma: 75 on 2016-04-29

Post score: 1

Original comments

Comment by croesmann on 2016-05-09:
According to your new edit: the issue is not related to your original question, but it is much more concerned with: how to compile a package from source. If you cannot see the parameter, you do not have Version 0.4 properly installed. Of course, you shouldn't have two copies of the package at once.

Comment by croesmann on 2016-05-09:
Here is a similar question to the missing version. It still takes some time until new packages are synchronized in ROS.

Comment by croesmann on 2016-05-09:
You can find information on how to compile from source here. Delete any duplicate teb_local_planner package first! If you want to be 100% sure, also invoke sudo apt-get remove ros-indigo-teb-local-planner first.


2 Answers 2


Rosanswers logo

To my knowledge, move_base is currently not intended to provide / follow a global plan endlessly, since it switches between three states: Planning (globally), Controlling (locally, determining velocity commands) and Clearing. Obviously, you require a goal point for the global plan.

You can try the following:

First, we follow the 2 loops once:

Write your own global planner that computes your "predefined" plan of the 8-shape. Create your two loops by choosing an appropriate discretization (maybe every 0.2/0.3 cm, or better with a parameter). Of course, you should generate your intermediate points computer-aided. Writing a global planner is not hard. You must implement the required interface and export your compiled library as plugin. A tutorial can be found here: Writing A Global Path Planner As Plugin in ROS. You can also check out the carrot_planner example (click here for indigo source).

Then try which of your local planners performs best. Version 0.4 of the teb_local_planner provides a path following mode which could be really helpful here!

After configuring your planners for move_base, you must invoke move_base by sending a navigation goal in order to switch the state machine to the Planning state.

Multiple loops:

  • If you want to follow the path endlessly, you can write a small node that renews the navigation goal, if the robot has reached its goal. But here you will always have a deceleration and acceleration phase each time.
  • Maybe you can renew your global plan more often causing the state machine to switch between states planning and controlling. But this shouldn't be a problem since this is common practice with the default GlobalPlanner (parameter planning_frequency). This way it should be possible to follow the path endlessly without the decelaration and acceleration phase. But you might need to adjust your predefined path (incl goal) according to the current robot position.
  • If you just want to repeat a few times, you could replicate your global plan in the global planner multiple times (e.g. with a parameter).

Originally posted by croesmann with karma: 2531 on 2016-04-29

This answer was ACCEPTED on the original site

Post score: 3

Original comments

Comment by murdock on 2016-04-30:
Correct me if I am wrong, but from what I gathered I may just be able to use teb_local_planner's path following mode and that would solve my problem?

P.S. As always, thank you for your answers.

Comment by croesmann on 2016-05-02:
yeah, but in conjunction with a global planner which provides your predefined path. You can built upon the source code posted by Steven_Daniluk.


Rosanswers logo

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){
      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;
      // 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");
      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
      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
    // 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 % 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
    }// end for
    ROS_DEBUG("End point #%d, X=%.3f, Y=%.3f", index + 1, path_x_[index], path_y_[index]);

    return (true);
  }// end makePlan

Originally posted by Steven_Daniluk with karma: 606 on 2016-05-01

This answer was NOT ACCEPTED on the original site

Post score: 2

Original comments

Comment by aarontan on 2018-06-29:
hello, could you provide an example of the text file with coordinates? Id like to see the correct format


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.