[lane_select]getClosestWaypointNumberの算出方法改良

[nyxrobotics]

概要
ドキュメントがないため一部推測。

該当箇所は下記

autoware_ai_planning/lane_planner/nodes/lane_select/lane_select_core.cpp

Line 728 in f4633fb

int32_t getClosestWaypointNumber(const autoware_msgs::Lane &current_lane, const geometry_msgs::Pose &current_pose,

下記で「すでに通った場所」は最近接点の候補から除外している。

autoware_ai_planning/lane_planner/nodes/lane_select/lane_select_core.cpp

Line 746 in f4633fb

range_min = static_cast<uint32_t>(previous_number);

下記で一定より先のwaypointを候補から除外するような処理をしている。
ここで現在速度*3をintにキャストして区間としている理由は不明。

autoware_ai_planning/lane_planner/nodes/lane_select/lane_select_core.cpp

Lines 747 to 752 in f4633fb

	double ratio = 3;
	double dt = std::max(current_velocity.linear.x * ratio, static_cast<double>(search_closest_waypoint_minimum_dt));
	if (static_cast<uint32_t>(previous_number + dt) < current_lane.waypoints.size())
	{
	range_max = static_cast<uint32_t>(previous_number + dt);
	}

以上により前回のwaypoint(previous_number)から数え始め、一定区間内のwaypointを参照して一番近い点を求めている。
区間を絞っているのはおそらく経路が重なる点などでショートカットしないようにするため。
しかし区間サイズの決定方法が不明で、区間サイズが長いとショートカットする挙動になる。

目的
経路のジャンプを防ぐため。

提案内容
以下のような方法で各waypointと現在位置の距離を比較し、直近で「接近→離脱」に切り替わる部分をclosest_waypointとすれば例外なく線形に経路追跡できます。区間は今のままでも可。変えるなら合計距離が一定になるように確保するのが無難だと思います。

autoware_ai_planning/lane_planner/nodes/lane_select/lane_select_core.cpp

Lines 786 to 787 in f4633fb

	int32_t closest_waypoint_idx = idx_vec.at(static_cast<uint32_t>(std::distance(dist_vec.begin(), itr)));
	return closest_waypoint_idx;

↓変更後

  if (previous_number == -1)
  {
    return idx_vec.at(static_cast<uint32_t>(std::distance(dist_vec.begin(), itr)));
  }
  // Start searching from the previous point on the current path.
  // The point at which the distances begin to diverge is the next point.
  // This prevents jumping to another path near the current position.
  uint32_t closest_waypoint_idx = idx_vec.at(0);
  double closest_distance = dist_vec.at(0);
  if (idx_vec.size() > 1)
  {
    for (uint32_t i = 0; i < idx_vec.size(); i++)
    {
      if (dist_vec[i] > closest_distance)
      {
        break;
      }
      closest_waypoint_idx = idx_vec.at(i);
      closest_distance = dist_vec.at(i);
    }
  }
  return closest_waypoint_idx;

タスク

• closest_waypointを使用している機能を確認
• getClosestWaypointNumberの算出方法改良

[nyxrobotics]

上記解決できてなさそうなので調査中

[nyxrobotics]

下記コールバックがほぼ毎回呼ばれており、previous_number == -1になる→これまでに通った経路を無視して常に一番近い点を出力する

autoware_ai_planning/lane_planner/nodes/lane_select/lane_select_core.cpp

Lines 620 to 637 in f4633fb

	void LaneSelectNode::callbackFromLaneArray(const autoware_msgs::LaneArrayConstPtr &msg)
	{
	tuple_vec_.clear();
	tuple_vec_.shrink_to_fit();
	tuple_vec_.reserve(msg->lanes.size());
	for (const auto &el : msg->lanes)
	{
	auto t = std::make_tuple(el, -1, ChangeFlag::unknown);
	tuple_vec_.push_back(t);
	}
	lane_array_id_ = msg->id;
	current_lane_idx_ = -1;
	right_lane_idx_ = -1;
	left_lane_idx_ = -1;
	is_new_lane_array_ = true;
	is_lane_array_subscribed_ = true;
	}

/traffic_waypoints_arrayは/lane_ruleが最初に一回、/lane_stopが10Hzでパブリッシュしている

[nyxrobotics]

実装を見る限り「レーンが変化したらprevious_numberを-1にして初期化」したそう。
/lane_ruleノードからトピックを受け取る想定と思われる。
/traffic_waypoint_arrayでトピック名が重複して誤動作している可能性あり。

[nyxrobotics]

bugfix/nearest_pointにて作業。
/lane_stopがパブリッシュするトピック名を変更し検証。
想定した位置にclosest_waypointが反映されるようになった。

[Alpaca-zip]

経路がジャンプしてしまう件についてお力添えいただきありがとうございます。
竹芝でのrosbagで検証したところ、経路がジャンプしないことを確認することができました。

修正していただいた以下の件ですが、waypointの順番通りに進むという観点ではuint32_t closest_waypoint_idx = idx_vec.at(0);のみで十分な気がしてしまうのですが、819~831行目の処理についてどのような役割があるのか後ほどご教授願えますでしょうか。

autoware_ai_planning/lane_planner/nodes/lane_select/lane_select_core.cpp

Lines 818 to 831 in 88d74b6

	uint32_t closest_waypoint_idx = idx_vec.at(0);
	double closest_distance = dist_vec.at(0);
	if (idx_vec.size() > 1)
	{
	for (uint32_t i = 0; i < idx_vec.size(); i++)
	{
	if (dist_vec[i] > closest_distance)
	{
	break;
	}
	closest_waypoint_idx = idx_vec.at(i);
	closest_distance = dist_vec.at(i);
	}
	}

[nyxrobotics]

トピック名の変更と計算方法の変更で問題が２個あるので、先に対処したいトピック目の変更を別issueで立てます