Ask Your Question
0

Global Path Planner Plugin with Oscillating Husky and Flickering Path Issue

asked 2018-07-02 17:38:17 -0500

aarontan gravatar image

updated 2018-07-04 16:14:29 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-07-04 16:03:04 -0500

aarontan gravatar image

updated 2018-07-04 16:05:19 -0500

I have discovered that the size of the local costmap and the distance between each way point impacts the global plan. In this case, the local costmap was too big which seems to include too many waypoints therefore confusing the robot. With that being said, I am still looking for an official answer if anyone knows something about this.

I have improved this to follow a circle and adjusted the local costmap size to something smaller and it solved the flickering path issue ... here is a video and a follow up question

edit flag offensive delete link more

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

Stats

Asked: 2018-07-02 17:38:17 -0500

Seen: 102 times

Last updated: Jul 04 '18