Revision history [back]

You'll probably have to tweak your local planner to fix this. I'm still using pose_follower, and I made some changes just after:

while(fabs(diff.linear.x) <= tolerance_trans_ &&
      fabs(diff.linear.y) <= tolerance_trans_ &&
      fabs(diff.angular.z) <= tolerance_rot_)
{

^This is where the tolerances are checked. Originally there was:

if(current_waypoint_ < global_plan_.size() - 1)
{
  current_waypoint_++;
  tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
  diff = diff2D(target_pose, robot_pose);
}

^Notice, no stop command unless it's the last waypoint. Change the code so it does stop, regardless of whether it's the last waypoint:

goal_position = false;
    while(fabs(diff.linear.x) <= tolerance_trans_ &&
          fabs(diff.linear.y) <= tolerance_trans_ &&
          fabs(diff.angular.z) <= tolerance_rot_)
    {
      // We're close enough, despite not being at the final waypoint. Stop here.
      if(current_waypoint_ < global_plan_.size() - 1)
      {
        current_waypoint_++;
        tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
        diff = diff2D(target_pose, robot_pose);
        geometry_msgs::Twist empty_twist;
        cmd_vel = empty_twist; // Stop
      }
      else
      {
        ROS_INFO("Reached goal: %d", current_waypoint_);
        in_goal_position = true;
        break;
      }
    }

I'll try to make a PR for this, although I'm not sure how popular pose_follower is.

You'll probably have to tweak your local planner to fix this. I'm still using pose_follower, and I made some changes just after:

while(fabs(diff.linear.x) <= tolerance_trans_ &&
      fabs(diff.linear.y) <= tolerance_trans_ &&
      fabs(diff.angular.z) <= tolerance_rot_)
{

^This is where the tolerances are checked. Originally there was:

if(current_waypoint_ < global_plan_.size() - 1)
{
  current_waypoint_++;
  tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
  diff = diff2D(target_pose, robot_pose);
}

^Notice, no stop command unless it's the last waypoint. Change the code so it does stop, regardless of whether it's the last waypoint:

 goal_position = false;
    while(fabs(diff.linear.x) <= tolerance_trans_ &&
          fabs(diff.linear.y) <= tolerance_trans_ &&
          fabs(diff.angular.z) <= tolerance_rot_)
    {
      // We're close enough, despite not being at the final waypoint. Stop here.
      if(current_waypoint_ < global_plan_.size() - 1)
      {
        current_waypoint_++;
        tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
        diff = diff2D(target_pose, robot_pose);
        geometry_msgs::Twist empty_twist;
        cmd_vel = empty_twist; // Stop
      }
      else
      {
        ROS_INFO("Reached goal: %d", current_waypoint_);
        in_goal_position = true;
        break;
      }
    }

I'll try to make a PR for this, although I'm not sure how popular pose_follower is.

You'll probably have to tweak your local planner to fix this. I'm still using pose_follower, and I made some changes just after:

while(fabs(diff.linear.x) <= tolerance_trans_ &&
      fabs(diff.linear.y) <= tolerance_trans_ &&
      fabs(diff.angular.z) <= tolerance_rot_)
{

^This is where the tolerances are checked. Originally there was:

if(current_waypoint_ < global_plan_.size() - 1)
{
  current_waypoint_++;
  tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
  diff = diff2D(target_pose, robot_pose);
}

^Notice, no stop command unless it's the last waypoint. Change the code so it does stop, regardless of whether it's the last waypoint:

    goal_position = false;
    while(fabs(diff.linear.x) <= tolerance_trans_ &&
          fabs(diff.linear.y) <= tolerance_trans_ &&
          fabs(diff.angular.z) <= tolerance_rot_)
    {
      // We're close enough, despite not being at the final waypoint. Stop here.
      if(current_waypoint_ < global_plan_.size() - 1)
      {
        current_waypoint_++;
        tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
        diff = diff2D(target_pose, robot_pose);
        geometry_msgs::Twist empty_twist;
        cmd_vel = empty_twist; // Stop
      }
      else
      {
        ROS_INFO("Reached goal: %d", current_waypoint_);
        in_goal_position = true;
        break;
      }
    }

I'll try to make a PR for this, although I'm not sure how popular pose_follower is.this.

You'll probably have to tweak your local planner to fix this. I'm still using pose_follower, and I made some changes changes just after:

while(fabs(diff.linear.x) <= tolerance_trans_ &&
      fabs(diff.linear.y) <= tolerance_trans_ &&
      fabs(diff.angular.z) <= tolerance_rot_)
{

^This is where the tolerances are checked. Originally there was:

if(current_waypoint_ < global_plan_.size() - 1)
{
  current_waypoint_++;
  tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
  diff = diff2D(target_pose, robot_pose);
}

^Notice, no stop command unless it's the last waypoint. Change the code so it does stop, regardless of whether it's the last waypoint:

    goal_position = false;
    while(fabs(diff.linear.x) <= tolerance_trans_ &&
          fabs(diff.linear.y) <= tolerance_trans_ &&
          fabs(diff.angular.z) <= tolerance_rot_)
    {
      // We're close enough, despite not being at the final waypoint. Stop here.
      if(current_waypoint_ < global_plan_.size() - 1)
      {
        current_waypoint_++;
        tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
        diff = diff2D(target_pose, robot_pose);
        geometry_msgs::Twist empty_twist;
        cmd_vel = empty_twist; // Stop
      }
      else
      {
        ROS_INFO("Reached goal: %d", current_waypoint_);
        in_goal_position = true;
        break;
      }
    }

I'll try to make a PR for this.