aarontan's profile - activity

Hello,

I am trying to make a predefined path for my robot to follow.

From my question here, I was adviced to create a custom global planner plugin to define the path and allow the local planner to follow it. I have gone through the famous global planner plugin tutorial, and I am currently trying to implement the code that is posted as an answer to this question by @tianb03.

I am using the husky navigation package. When I give a final orientation in rviz, the path generated by me is visible but the husky seems to be stuck at its initial position. Once I give the husky a little push, it begins to follow the path towards the goal; however, this path quickly disappear and appear repeatedly, causing the husky to run around in circles...

Here is a link to a video illustrating my problem: https://www.youtube.com/watch?v=VvE5r...

I am wondering, why is this happening? How can I make the husky follow my predefined global path as close as possible before coming to a stop at the destination? (Do I always have to define the orientation? Can it just stop in its tracks as it nears the goal pose without correcting its orientation to a user defined input?)

Below is my implementation of the global planner plugin as suggested by @tianb03 (Based on the carrot planner cpp):

#include <carrot_planner/carrot_planner.h>
#include <pluginlib/class_list_macros.h>

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(carrot_planner::CarrotPlanner, nav_core::BaseGlobalPlanner)

namespace carrot_planner {

  CarrotPlanner::CarrotPlanner()
  : costmap_ros_(NULL), initialized_(false){}

  CarrotPlanner::CarrotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
  : costmap_ros_(NULL), initialized_(false){
    initialize(name, costmap_ros);
  }

  void CarrotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
      costmap_ros_ = costmap_ros;
      costmap_ = costmap_ros_->getCostmap();

      ros::NodeHandle private_nh("~/" + name);
      private_nh.param("step_size", step_size_, costmap_->getResolution());
      private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
      world_model_ = new base_local_planner::CostmapModel(*costmap_); 

      initialized_ = true;
    }
    else
      ROS_WARN("This planner has already been initialized... doing nothing");
  }

  //we need to take the footprint of the robot into account when we calculate cost to obstacles
  double CarrotPlanner::footprintCost(double x_i, double y_i, double theta_i){
    if(!initialized_){
      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
      return -1.0;
    }

    std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
    //if we have no footprint... do nothing
    if(footprint.size() < 3)
      return -1.0;

    //check if the footprint is legal
    double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);
    return footprint_cost;
  }


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

    if(!initialized_){
      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
      return false;
    }

    ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);

    plan.clear();
    costmap_ = costmap_ros_->getCostmap();

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

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