[aster_search] 途中で一定以上に姿勢が変化する経路・ゴール地点でスタート地点から１週回っている経路

[nyxrobotics]

下記のように、経路長や合計角度が小さく、最短で一回転する経路が選択される場合がある。

[ INFO] [1681390944.898855030, 3714.048000000]: Search time: 114.942496 [msec]
[ INFO] [1681390944.898893280, 3714.048000000]: move_distance= 16.728981,theta= -6.675884,move_angle= 7.984881

asterの算出する経路としては正しい。しかしより先のwaypointをゴール地点とすれば旋回のない経路を得られる。
thetaが2pi超える場合などを経路の候補から除外すると旋回の含まれる経路をスキップし、他のゴール地点で再計算する。
これにより旋回を含まない経路のみを選択する動作が可能。

Originally posted by @nyxrobotics in #24 (comment)

[nyxrobotics]

#25 (comment)

ゴール地点がもっと奥に設定されていればスムーズな経路が出たと思われる例
→ゴール地点を定数でオフセットしても、奥側にも障害物かあった場合に旋回経路で解を決定してしまう
→角度や経路長で制限をかけ、迂回経路をすべて「解なし」として他のゴール地点候補で再計算するほうが良い

[nyxrobotics]

#24 (comment)

[nyxrobotics]

Aster searchで一番近いゴール地点に到達できるか探索→見つからなければ少し離れた次の点→・・・
という形で探索しているが、一定区間をゴールとして探索できれば一回の計算で到達しやすいゴールを見つけられそう
（一番近い点を目指して一回転する経路を出す前に、少し離れたスムーズな経路を見つけたい）