utexas-bwi / eband_local_planner

ROS Local planner based on the eband approach. Based on the original implementation by Christian Connette and Bhaskara Marathi. This local planner has been adapted primarily for differential drive robots, but still supports the original holonomic drive controls.
http://wiki.ros.org/eband_local_planner
108 stars 83 forks source link

move_base dose not put SUCCEEDED status (move_base/status or move_base/result) #43

Open tosa-no-onchan opened 2 years ago

tosa-no-onchan commented 2 years ago

The flowing code(return goalreached;) dosen't work correctly.

bool EBandPlannerROS::isGoalReached(){
  ....
  return goal_reached_;
  ....
}

Why did you comment out next lines? from here // nav_msgs::Odometry base_odom; //... to here //return is_goal_reached;

bool EBandPlannerROS::isGoalReached()
{
  // check if plugin initialized
  if(!initialized_)
  {
    ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
    return false;
  }

  return goal_reached_;

  // // copy odometry information to local variable
  // nav_msgs::Odometry base_odom;
  // {
  //    // make sure we do not read new date from topic right at the moment
  //    boost::mutex::scoped_lock lock(odom_mutex_);
  //    base_odom = base_odom_;
  // }

  // geometry_msgs::PoseStamped global_pose;
  // costmap_ros_->getRobotPose(global_pose);

  // // analogous to dwa_planner the actual check uses the routine implemented in trajectory_planner (trajectory rollout)
  // bool is_goal_reached = base_local_planner::isGoalReached(
  //     *tf_, global_plan_, *(costmap_ros_->getCostmap()),
  //     costmap_ros_->getGlobalFrameID(), global_pose, base_odom,
  //        rot_stopped_vel_, trans_stopped_vel_, xy_goal_tolerance_,
  //     yaw_goal_tolerance_
  // );

  // return is_goal_reached;

}
tosa-no-onchan commented 2 years ago

I found some correct code.

eband_local_planner_ros.cpp

    // set global plan to wrapper and pass it to eband    
bool EBandPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{
 ....
  // set goal as not reached
  // changed by nishi
  //goal_reached_ = false;

  return true;
}

bool EBandPlannerROS::isGoalReached()
{
  // check if plugin initialized
  if(!initialized_)
  {
    ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
    return false;
  }

  // added by nishi 
  //ROS_INFO("GOAL Reached!");
  //std::cout << "BandPlannerROS::isGoalReached() :#9  goal_reached_=" << goal_reached_ << std::endl;
  if(goal_reached_==true){
    goal_reached_=false;
    return true;
  }
  return false;

  //return goal_reached_;
  ....