Fix/tsukuba navigation

[shunki1006]

PR Type

• Feature
• Bug fix
• Refactor
• Documentation
• Other

Overview

• つくちゃれ2023左側スタート用ファイルの初期位置調整, ロボットのfootprint調整, stopするはずのwaypointで停止しない問題対策のNav2param調整.

Detail

1. つくばチャレンジ2023で左側スタートの際のロボットの初期位置を変更しました. 走行位置のスタート位置のタイルが1枚60cmで7枚分離れていたため, 4.2mずらしました.
2. 下記PRで急な障害物を確実に避けるために余分に大きくしたfootprintが大きすぎることが原因で狭い道を通ることができないことがつくばチャレンジ走行中に発覚したため, 下記PR変更前のfootprintに戻しました.
   参考；Fix/nav2 local costmap #73 (comment)
3. tandemが終わる位置にstopのパラメータがtrueになっているwaypointがあるとstopせずに通過してしまう問題があり, その原因として, navigation2のゴール到達判定と, waypoint_navigationのゴール到達判定の相違にあると考えたため, Nav2param内の到達判定を厳しく, waypoint_navigation内の到達判定を緩くすることで対応しました. これにより, 基本的waypoint_navigationの判定をもとに次の動作をするようになっています.

Test

• つくばチャレンジ動作確認済み.

Attention

• つくちゃれ左側スタート用に初期位置を変えた点と, launch内で指定する走行コースがつくばようになっている以外は下記PRと同様の内容になっているはずです.
  参考；Fix/nav2 param navigation #82 (comment)

[Alpaca-zip]

waypointがあるとstopせずに通過してしまう問題があり, その原因として, navigation2のゴール到達判定と, waypoint_navigationのゴール到達判定の相違にあると考えたため

これとtandemは何か関係があるんですか？

[koki0624]

3. tandemが終わる位置にstopのパラメータがtrueになっているwaypointがあるとstopせずに通過してしまう問題があり, その原因として, navigation2のゴール到達判定と, waypoint_navigationのゴール到達判定の相違にあると考えたため, Nav2param内の到達判定を厳しく, waypoint_navigation内の到達判定を緩くすることで対応しました. これにより, 基本的waypoint_navigationの判定をもとに次の動作をするようになっています.

返信が遅れました。
ここの記述ですが、以下の2つの問題（原因は推測です）が複合しています。

1. stop が true である waypoint のゴール判定は、距離と方位が waypoint_nav にパラメータで与える min_dist_err, min_yaw_err を下回るか、nav2 の方でゴールしたと判定されるかのどちらかを満たすときです。しかし、stop が true である waypoint にゴールを設定した直後、後者の nav2 のゴール判定が残っている（nav2 はゴールした状態のままになっている）ことがあるようで、1つ前の waypoint で、停止してしまうことがありました。仮にここで手動で再開させたとしても、本来停止すべき地点を通過してしまうということが起きました。

2. tandem が終わる地点では、global_costmap の状態の切り替え（tanndem中は無効化、終わると有効化）を行なっています。ただ、ナビゲーション中に行なうと不具合があったので、一度停止させてから切り替え、再開させています。この再開させるタイミングが、stop が true の waypoint と被ると、手動で再開されたものと区別ができず、勝手にスタートしてしまうのではないか、と推測していました。

対応として、1に対しては nav2 のゴール判定を厳しくすることで nav2 のゴール判定よりも先に waypoint_nav にゴール判定をさせることで対応しました。2に対しては、一時停止直前の tandem を無くすことで対応しました。

厳密な検証をする時間はなかったので、原因は推測に過ぎませんが、ひとまず本番では以上の対応をしました。

[Alpaca-zip]

1つ前の waypoint で、停止してしまうことがありました。仮にここで手動で再開させたとしても、本来停止すべき地点を通過してしまうということが起きました。

waypoint_navigation側ではゴール到達判定になっているが、navigation2ではゴール到達判定になっていない状態で、navigation2の方でゴールしたと判定されると、waypoint_navigation側では一つ先のwaypointでゴールしたことになってしまい停止したいwaypointで停止できないということですか？

この再開させるタイミングが、stop が true の waypoint と被ると、手動で再開されたものと区別ができず、勝手にスタートしてしまうのではないか、と推測していました。

手動で再開するときって/joyを監視しているのではないですか？

[koki0624]

1つ前の waypoint で、停止してしまうことがありました。仮にここで手動で再開させたとしても、本来停止すべき地点を通過してしまうということが起きました。

waypoint_navigation側ではゴール到達判定になっているが、navigation2ではゴール到達判定になっていない状態で、navigation2の方でゴールしたと判定されると、waypoint_navigation側では一つ先の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 の状態も利用しているため）
4. 結果として N-1 番目の waypoint に到達直後、N番目の waypoint に到達したと判定されてしまい、一時停止する。

このようなプロセスで問題が起きていると推測しています。

この再開させるタイミングが、stop が true の waypoint と被ると、手動で再開されたものと区別ができず、勝手にスタートしてしまうのではないか、と推測していました。

手動で再開するときって/joyを監視しているのではないですか？

ナビゲーション時、コントローラは使用していません。手動とは、rviz 上のボタンで再開させるという意味です。

[Alpaca-zip]

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 の状態も利用しているため）
4. 結果として N-1 番目の waypoint に到達直後、N番目の waypoint に到達したと判定されてしまい、一時停止する。

このようなプロセスで問題が起きていると推測しています。

なるほど、理解しました。
waypoint_nav側で、どのwaypointのゴール判定であるかの識別はできたほうが良さそうですね。この件について時間があればissue化していただけると嬉しいです。

ナビゲーション時、コントローラは使用していません。手動とは、rviz 上のボタンで再開させるという意味です。

すみません。完全に忘れていました。
rviz 上のボタンで再開させるのとtanndemから復帰する際の区別がつかないということですが、何かグローバル変数などでステートが共有されてしまっているということですか？

[koki0624]

なるほど、理解しました。 waypoint_nav側で、どのwaypointのゴール判定であるかの識別はできたほうが良さそうですね。この件について時間があればissue化していただけると嬉しいです。

承知です。後ほど orange_navigation の方で issue にしておきます。

すみません。完全に忘れていました。 rviz 上のボタンで再開させるのとtanndemから復帰する際の区別がつかないということですが、何かグローバル変数などでステートが共有されてしまっているということですか？

ナビゲーションの中断や再開は、すべて waypoint_nav の ROS Service をコールする形で実装しています（rviz や tandem からこのサービスをコールする）。このとき、現状の実装では waypoint_nav は誰からサービスをコールされたのか判別ができない、ということです。

ROS2 移行時に考えてはいたのですが、ROS1 ではこれが問題になることがなかった（これが問題の原因かは不確かですが）ということと、ROS2の標準サービス（std_srvs）では、リクエスト側にデータを含むものが Bool のみで、2つ以上の判別ができない（十分だとしてもあまり美しくない）こともあり先送りにしていました。

[Alpaca-zip]

ナビゲーションの中断や再開は、すべて waypoint_nav の ROS Service をコールする形で実装しています（rviz や tandem からこのサービスをコールする）。このとき、現状の実装では waypoint_nav は誰からサービスをコールされたのか判別ができない、ということです。

ROS2 移行時に考えてはいたのですが、ROS1 ではこれが問題になることがなかった（これが問題の原因かは不確かですが）ということと、ROS2の標準サービス（std_srvs）では、リクエスト側にデータを含むものが Bool のみで、2つ以上の判別ができない（十分だとしてもあまり美しくない）こともあり先送りにしていました。

解説頂きありがとうございます。こちらの件もissue対応した方が良いかもしれませんね。

とりあえずここでの作業内容は良いと思います。