Custom Global Planner Plugin with Obstacle Avoidance Problems

edit

Hello,

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: https://www.youtube.com/watch?v=Wx0SQ...

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: https://www.youtube.com/watch?v=QmdPG...

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++){ 

    x_[w]=increment;
    y_[w]=0;
    increment = increment + radius/step;
    for_loop++;

   }

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