Is there a way to give the planner a pre-planned global path?

[diegoguffanti]

I have a global path and want the local plan to follow this path. Is it possible to do this using the teb_local_planner?

[jcmonteiro]

This is precisely what TEB is used for. Please read the docs.

[diegoguffanti]

I'm sorry, I think the question was asked in the wrong way. I tried to ask for the use of a custom path passed to TEB as an input. Not the one generated when publishing a Goal. I have a path message that stores a series of points building for example a circle. How can I do the TEB planner follow these points?

[jcmonteiro]

Hey, Diego. Sorry to sound a little rude, but this is really what TEB is used for.

You did not give much information, but I believe that what you are looking for is using TEB as a plugin. Either as a nav_core::BaseLocalPlanner or a mbf_costmap_core::CostmapController plugin (there is support for both). Maybe it is easier if you to start here http://wiki.ros.org/nav_core.

[diegoguffanti]

Thanks for your response. I must say you that It is precisely what I do. I use the TEB as a pluging. It is configured using the base local planner parameter of move_base node. However your response helped me to learn how to modify the global planner to provide a custom path to the local planner (TEB) (this was what I asked). Can you recommend me a way to generate this custom path through via-points?. The TEB has implemented this functionality but it doesn't work.

[jcmonteiro]

No problem, Diego. What about generating your global plan multiple times to go through the viapoints if you really want to reach them?

[diegoguffanti]

I don't want to do it because I would have to re-plan each time I reached a viapoint and the movement would not be continuous.

I followed this tutorial (http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS) to set up the global planner plugin. If you look, the points that form the global path are calculated within GlobalPlanner::makePlan just for illustration purposes.

How can I subscribe to a topic for example to /clickedpoint from RVIZ to draw the path myself and pass it to the plugin to work with these points? I can't subscribe to any topic from the plugin.

[jcmonteiro]

I don't want to do it because I would have to re-plan each time I reached a viapoint and the movement would not be continuous.

If your viapoints are known beforehand, you can always call the global planner for each consecutive pair and then feed this complete path to TEB. If a viapoint is received while moving, then just replan when that happens.

How can I subscribe to a topic for example to /clickedpoint from RVIZ to draw the path myself and pass it to the plugin to work with these points? I can't subscribe to any topic from the plugin.

The local planner is used as a plugin. So the one responsible for subscribing to a topic is the node that is making use of the plugin.

[diegoguffanti]

Thanks @jcmonteiro !. But how I can feed this complete path to TEB?. That's exactly why I'm thinking about how to subscribe to a topic from the plugin

[jcmonteiro]

No problem. You just need to call the method TebLocalPlannerROS::setPlan in your TEB instance to update the global plan whenever there is a new one available. Then, your next call to TebLocalPlannerROS::computeVelocityCommands will already return the velocity as a solution to the updated plan.

[diegoguffanti]

After a hard day of work and once several issues of compilation were solved, I was able to build the teb_local_planner plugin from source. Before this I was using the pre-built version of TEB. I have seen the methods that you tell me... I will try to do what you recommend and I will tell you. Thanks!

[diegoguffanti]

I added a new topic in TEB to receive the new path. When using the TebLocalPlannerROS::setPlan method, the local planner begins to follow the path that I design, however I have seen that 2 paths are being received, the one generated by me and the one that comes from the move_base node. How can I stop the plan that comes from the move_base node? Additionally you told me to call the TebLocalPlannerROS::computeVelocityCommands method but this method receives as argument cmd_vel ... how can i call it correctly?

[diegoguffanti]

@jcmonteiro let me know if you know how I can stop the plan coming from the move_base node. As I said you before, 2 global plans are being received, the one generated by me and the one that comes from the move_base node. Thank you for your help

[ryleymcc]

So I think I am doing the same thing as you. I create a map to localize in with rtabmap_ros, then I run a my custom python script (I call it Path Creator) that subscribes to the /rtabmap/localization_pose topic and saves the translation(x,y) and orientation(x,y,z,w) to a text file. Then I followed this tutorial to create my own global plugin that I call GlobalPathInjector(GPI) that reads from the text file created by Path Creator when you trigger a goal. The goal/start pose(xyz,xyzw) does not get used. It's only to trigger the GPI plugin). This should be pretty easy for anyone to replicate on their own using the tutorial. You could also look at this question

To answer your last question. To make move_base use your custom plugin as the global planner you add it to your move_base launch node like this.

......
 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
 <param name="base_global_planner" value="my_custom_global_planner_name/MyCustomGlobalPlannerName"/>
 ....

The only function I modified from the tutorial was this part. (also had to change the names to my_custom_global_planner_name and MyCustomGlobalPlannerName wherever I needed to)

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

    plan.push_back(start);
   for (int i=0; i<20; i++){
     geometry_msgs::PoseStamped new_goal = goal;
     tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);

      new_goal.pose.position.x = -2.5+(0.05*i);
      new_goal.pose.position.y = -3.5+(0.05*i);

      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);
   }
   plan.push_back(goal);
  return true;

[JohnTGZ]

If you would like a more robust way and less hacky way to use TEB Planner to execute a pre-planned path, you can check out Move Base Flex, it provides a more granular API on top of the normal Move Base node.