Ask Your Question

Custom Global Planner Plugin with Obstacle Avoidance Problems

asked 2018-07-04 15:58:26 -0500

aarontan gravatar image

updated 2018-07-05 16:28:05 -0500


UPDATE: July 5th

I have simplified the predefined path to just a straight line instead of a circle as seen in my original question (see below). I have placed an obstacle in front of the husky to further investigate this obstacle avoidance issue. I have found that when the obstacle is placed within the local costmap, the lcoal planner will attempt to avoid the obstacle; however, never successfully. If there is an obstacle that is originally outside of the local costmap, as the robot moves closer to it (as the obstacle comes into the local costmap), the DWA planner will fail to produce a path...

Here is a video describing this:

I am wondering, why is this?

From the video, you will also see that, once i remove the second obstacle, the husky will start behaving weirdly, I am also not sure why this happens. The global path also starts flickering.

ORIGINAL: July 4th

I have created my own custom global planner plugin as described by this tutorial. Instead of what's described in the tutorial, I wrote something to enable my mobile robot to follow a predefined path, in this case a circle. This code will first drive the robot from its original position to a point on the circle with a predefined radius and then circle around this circular path. As it stands now, there are no obstacle avoidance.

My question is, how do I implement obstacle avoidance to this plugin? If an obstacle were to appear on this predefined path, I'd like my robot to maneuver around the obstacle and continue on the same circular path.

Here is a video link showing my situation:

The following is my global planner plugin:

#include <pluginlib/class_list_macros.h>
#include "global_planner.h"

 //register this planner as a BaseGlobalPlanner plugin
 PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

 using namespace std;

 //Default Constructor
 namespace global_planner {

 GlobalPlanner::GlobalPlanner (){


 GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
   initialize(name, costmap_ros);

 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){


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

    double radius = 7; //radius of circle
    double resolution = 0.25; //resolution of circle in radians
    double index = resolution; //how much to increment at each point on the circle
    int full_circle = 6/resolution; //how many increments to finish the circle eg. 6 radians is a full circle

    int step = 5; //resolution of the straight part from center to point on circle
    int for_loop=0; //array location counter
    double increment = radius/step; 
    int size = step + full_circle;      

    double x_[size]={0};
    double y_[size]={0};
    double angle;

   //coordinates from center to a point on the circle
   for( int w = 0; w < step; w++){ 

    increment = increment + radius/step;


   //coordinates from the point on the circle to the full circle
   for( int s = for_loop; s < size + 1; s++){

    angle = angle + index;
    x_[s] = radius ...
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted

answered 2018-07-05 18:24:07 -0500

ahendrix gravatar image

The local planner may be able to avoid some obstacles, but it won't be able to avoid all of them. When it encounters an situation where it cannot make forward progress, it will abort and the global planner will be asked to re-plan. When this happens you'll see DWA Planner failed to produce a path and Got new plan messages. In this circumstance, your global planner should generate a new global plan that avoids the obstacle. If you have your costmaps set up correctly, move_base will update both the global and local costmaps with the obstacle, and when the global planner is called to produce a new plan, it will get the updated global costmap that has the new obstacles in it.

From your video, it looks like you may have some issues with your local planner configuration. I see two behaviors that both look like configuration issues:

  • Around 0:12 to 0:33, the Husky rotates back and forth in place while trying to get around the obstacle. It looks like the local planner may be trying to command a combination of strafing and angular velocity, but since the husky cannot move sideways it only executes and angular part of the motion.
  • From 1:04 to the end of the video, it looks like the Husky is near the goal and is trying to achieve the goal position more accurately. Again, it may be trying to execute a strafing maneuver.

I would start by ploting the fields in the cmd_vel topic to see what commands are sent, particularly when the robot is stuck.

Since ClearPath provides a local planner configuration for the Husky, I would try to find that configuration and use it. I suspect that will solve most of your issues. If you need to do more troubleshooting, I would start by checking the holonomic field in the configuration (make sure it's turned off) and then tune the linear and angular velocity control parameters.

edit flag offensive delete link more

answered 2018-07-05 12:59:02 -0500

Are you using this with the navigation stack already? If so, you're in luck, most of the hard work is done for you for at least an MVP of your project!

So move base will ask you to replan at whatever rate you specify in your move base params file, so over time you should be planning with the new collision information. The local planner if it fails to compute a path (because something is in the way) will also trigger the global planning plugin to recompute.

It's just up to you in your application to use that Costmap2DROS pointer to get the costs of your path and make sure they're valid and if not, go around them or stop.

edit flag offensive delete link more


yes, I am using this with the husky_navigation stack already. I can see both the local/global costmaps (as shown in the attached video) in rviz. I am not sure how to use the costmap2dros information, I'd like the global path to stay the same, is it possible for dwa to avoid the obstacle?

aarontan gravatar image aarontan  ( 2018-07-05 14:07:21 -0500 )edit

DWA will do its best but at some point you HAVE to replan around large obstacles, dwa isn't magic, if you tell it to go through a wall, its not going to have much of a chance.

stevemacenski gravatar image stevemacenski  ( 2018-07-05 17:59:29 -0500 )edit

I see, so I have to be able to make the global planner able to replan. I have opened a separate question regarding this here. If you know anything, could you help me there? (I am not sure if this counts as xposting, if so, sorry).

aarontan gravatar image aarontan  ( 2018-07-09 09:18:41 -0500 )edit

answered 2018-07-04 22:19:18 -0500

Saurav Agarwal gravatar image

updated 2018-07-04 22:20:32 -0500

You can use the costmap_2d ros package which gives you collision checking ability. The costmap_2d package provides an API for you to use in conjunction with your planner for obstacle avoidance by checking the robot's footprint cells for collision in a global costmap for planning. For local obstacle avoidance it also provides a rolling window costmap.

edit flag offensive delete link more


Thank you for your reply, do you have any examples that you can recommend? I am looking at other global planners and how they replan the global path when an obstacle arises but I cannot find a global planner that allows predefined paths with obstacle avoidance

aarontan gravatar image aarontan  ( 2018-07-05 11:09:43 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2018-07-04 15:58:26 -0500

Seen: 1,097 times

Last updated: Jul 05 '18