The flowing code(return goal_reached_;) dosen't work correctly.
bool EBandPlannerROS::isGoalReached(){
....
return 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;
}