stop を true に設定した waypoint のゴール判定が適切に行われないときがある（humble-devel）

[koki0624]

※ この問題が確認されたのは humble-devel のみです。

問題の概要

一時停止をするために stop を true に設定した waypoint の一つ手前の waypoint で一時停止をしてしまうという問題が、しばしば起きることが確認されました。
検証が不十分ですが、現時点では以下のようなプロセスで問題が発生していると推測しています。

---

stop が true の waypoint を N番目としたとき

1. N-1番目の waypoint に到達する。このとき、waypoint_nav はもちろん、nav2 の方でもゴールしたと判定された場合、nav2 の状態が "ゴールした" 状態になる。
2. waypoint_nav が N番目の waypoint をゴールに設定し、ゴール判定をし始める。
3. nav2 の状態が "ゴールした" 状態のままの場合、N番目にゴールを設定した直後に waypoint_nav はゴールしたと判定してしまう。（stop が true の waypoint でのゴール判定に nav2 の状態も利用しているため）

結果として N-1 番目の waypoint に到達直後、N番目の waypoint に到達したと判定されてしまい、一時停止する。

現状の対応

nav2 ゴール判定 を waypoint_nav の判定よりも厳しく設定し、nav2 の状態が "ゴールした" 状態になることを防ぐことで、この問題は発生しなくなるようです。

しかし、この問題は特定の箇所のみで確認できたため、発生する条件に別の要因が絡んでいる可能性があり、要検証です。

[Alpaca-zip]

この問題が発生しているとみられる箇所は以下です。

orange_navigation/waypoint_navigation/src/waypoint_nav.cpp

Lines 337 to 351 in 8aeaae1

	bool WaypointsNavigation::onNavPoint(const geometry_msgs::msg::Pose& goal_pose)
	{
	const geometry_msgs::msg::Pose robot_pose = current_pose_;
	float x_diff = goal_pose.position.x - robot_pose.position.x;
	float y_diff = goal_pose.position.y - robot_pose.position.y;
	float dist = std::sqrt(x_diff * x_diff + y_diff * y_diff);
	float dist_err = waypoint_list_[wp_num_ - 1].rad;
	if (waypoint_list_[wp_num_ - 1].stop)
	{
	double yaw_diff = std::abs(target_yaw_ - getYaw(robot_pose.orientation));
	return ((dist < min_dist_err_) && (yaw_diff < min_yaw_err_)) ||
	(nav_status_ == rclcpp_action::ResultCode::SUCCEEDED);
	}
	return dist < dist_err;
	}

radがNav2のゴール判定より大きいことが前提ですが、以下のようにひとつ前のウェイポイントとの距離がwaypoint_list_[wp_num_ - 2].radより小さい場合にはnav_status_ == rclcpp_action::ResultCode::SUCCEEDEDを評価しない様にすれば直りそうな気がします。

bool WaypointsNavigation::onNavPoint(const geometry_msgs::msg::Pose& goal_pose)
{
  const geometry_msgs::msg::Pose robot_pose = current_pose_;
  float x_diff = goal_pose.position.x - robot_pose.position.x;
  float y_diff = goal_pose.position.y - robot_pose.position.y;
  float dist = std::hypot(x_diff, y_diff);
  float dist_err = waypoint_list_[wp_num_ - 1].rad;

  if (waypoint_list_[wp_num_ - 1].stop)
  {
    float x_diff_prev = waypoint_list_[wp_num_ - 2].x - robot_pose.position.x;
    float y_diff_prev = waypoint_list_[wp_num_ - 2].y - robot_pose.position.y;
    float dist_prev = std::hypot(x_diff_prev, y_diff_prev);
    float dist_err_prev = waypoint_list_[wp_num_ - 2].rad;
    double yaw_diff = std::abs(target_yaw_ - getYaw(robot_pose.orientation));

    if (dist_prev < dist_err_prev)
      return (dist < min_dist_err_) && (yaw_diff < min_yaw_err_);

    return ((dist < min_dist_err_) && (yaw_diff < min_yaw_err_)) ||
           (nav_status_ == rclcpp_action::ResultCode::SUCCEEDED);
  }
  return dist < dist_err;
}

[koki0624]

なるほど、1つ前のウェイポイントのゴール判定距離内にいる間は、nav2 の状態を確認しない、ということですね。

ありだと思うのですが、1つ前のウェイポイントのゴール判定距離内に、次のウェイポイント（stop が true）があるような場合（nav2 の状態を確認しない）が気になります。

stop が true のウェイポイントで nav2 の状態を確認しない場合、min_dist_err, min_dist_yaw によるゴール判定よりも nav2 のゴール判定（xy_goal_tolerance, yaw_goal_tolerance）の方が厳しい必要があります。
でないと、nav2 ではゴール判定されたのでロボットはそれ以上動かないにも関わらず、waypoint_nav ではゴール判定していないので、一時停止状態にならない、ということになってしまいます。
これは現状の対応と同様です。

[Alpaca-zip]

なるほど。そもそも、waypoint_navigationとnavigation2でゴール判定が違うのが事をややこしくしていそうですね。

navigation2ではゴール判定を動的に変えられるようになっているので(以下参照)、waypoint_navigation側でxy_goal_toleranceおよびyaw_goal_toleranceをsetしてnav_status_ == rclcpp_action::ResultCode::SUCCEEDEDのみで判定するようにすればこの問題も楽に解決しそうですね。
https://github.com/ros-planning/navigation2/blob/4bc3e8a534005e983ee7a49a09ec00aeb9820661/nav2_controller/plugins/simple_goal_checker.cpp#L144-L166

[koki0624]

確かに、パラメータの動的変更は盲点でした。
とすれば、xy_goal_tolerance に rad を、yaw_goal_tolerance には $\pi$ とか $2\pi$ を設定して、stop が true のときは min_yaw_err の値を設定すればいけるかもですね。

[Alpaca-zip]

確かに、パラメータの動的変更は盲点でした。 とすれば、xy_goal_tolerance に rad を、yaw_goal_tolerance には π とか 2π を設定して、stop が true のときは min_yaw_err の値を設定すればいけるかもですね。

ですね。暇な時があれば対応しておきます。