ROS2用waypointパッケージ作成

[masakifujiwara1]

Description

• interactiveなwaypoint配置
• menu機能の拡張
• stop機能の作成
• waypoint_followrからの脱却 or パラメータ変更

[masakifujiwara1]

作業リポジトリ
https://github.com/masakifujiwara1/waypoint_manager2.git

[masakifujiwara1]

現状, ストップ機能以外はGUI上でwaypointを問題なく動かすことができる.

[masakifujiwara1]

stop機能を追加し, bug取りも分かっている範囲では終了した. 今後, 津田沼キャンパスで予行演習した際に, 問題ないか確認する必要がある.

[masakifujiwara1]

to do...

• 予行演習（津田沼キャンパス）
• リファクタリング
• C++に移植作業
• [ ]

[masakifujiwara1]

0920
津田沼2号館3階で走行させた. waypointを1つ飛ばしてしまうことが3回中1回確認された.

[masakifujiwara1]

0923の実験走行にて，waypointを飛ばしてしまうバグを発見．
また，ストップ機能を使用して止まる際のロボットの向きもwaypointの矢印通りになっていない．

[masakifujiwara1]

以下waypointを飛ばしてしまう挙動が見られた時のログ．

navigation_time : builtin_interfaces.msg.Duration(sec=87, nanosec=670042192)
number_of_recoveries : 0
distance_remaining : 53.615150451660156
False 12
navigation_time : builtin_interfaces.msg.Duration(sec=87, nanosec=680008535)
number_of_recoveries : 0
distance_remaining : 53.615150451660156
False 12
navigation_time : builtin_interfaces.msg.Duration(sec=87, nanosec=690088893)
number_of_recoveries : 0
distance_remaining : 53.615150451660156
False 12
navigation_time : builtin_interfaces.msg.Duration(sec=87, nanosec=700034924)
number_of_recoveries : 0
distance_remaining : 53.59014892578125
False 12
navigation_time : builtin_interfaces.msg.Duration(sec=87, nanosec=715660482)
number_of_recoveries : 1
distance_remaining : 0.0
False 12
[INFO] [1696573609.719691028] [waypoint_manager2_node]: Goal failed!
navigation_time : builtin_interfaces.msg.Duration(sec=0, nanosec=15207)
number_of_recoveries : 0
distance_remaining : 0.0
False 13
navigation_time : builtin_interfaces.msg.Duration(sec=0, nanosec=9768336)
number_of_recoveries : 0
distance_remaining : 0.0
False 13
navigation_time : builtin_interfaces.msg.Duration(sec=0, nanosec=19857650)
number_of_recoveries : 0
distance_remaining : 0.0
False 13

[masakifujiwara1]

上のログより，リカバリーが起こるか，GoalStatusがfailedになった際に，distance_remaining = 0.0になることが原因と推測．
詳細は，next_waypointする条件の1つである，Goal_radiusの中に入った判定になってしまうことで，next_waypointしてしまう．

[masakifujiwara1]

distance_remainingはpathの長さで計算されている．つまり，単純な位置関係での距離で計算されていない．このことから，waypointが切り替わる際に，大域的経路計画が遅いと，distance_remainingが新しい値に切り替わるのが遅れて，先に報告した問題が発生してしまう．これに対して，distance_remainingの他に/amcl_poseを用いた距離の計算を実装した．これにより，検証した限りでは問題は発生していない．

[masakifujiwara1]

ストップ機能を使用して止まる際のロボットの向きがwaypointの矢印通りになるように修正を行った．

[masakifujiwara1]

waypointのdelete機能に関しても，実装した．
masakifujiwara1/waypoint_manager2@06536c2

[masakifujiwara1]

必要な機能の実装は終わった．
今後は，実ロボットで使用しやすいようにlaunchやparamの整備を行う予定である．

[masakifujiwara1]

1022 実験走行

• stop waypointで停止する際に，visualizeされているwaypointの矢印の向き通りに停止した
• 前回（9月）の実験走行で起きたwaypointを飛ばしている問題の対処ができていた
• success goalとなってしまい，stop waypointでなくても停止してしまう問題の対処ができていた

起こった問題

• navigationは継続されるが，waypointのvisualizeが消えてしまう

[masakifujiwara1]

visualizeが消えてしまう問題を対処した．
masakifujiwara1/waypoint_manager2@9ceb17e

[masakifujiwara1]

信号認識用の機能を追加した．
masakifujiwara1/waypoint_manager2@e900ca1

バグの修正を行った．
masakifujiwara1/waypoint_manager2@d8a637e

https://github.com/masakifujiwara1/waypoint_manager2/tree/tsukuba2023-traffic-light

[masakifujiwara1]

以下の対策を行った．

• [waypoint]コーン対策 #41
• [waypoint] stop waypointから復帰する際のclear costmap #42