include goal tolerance in planning, not only when is near to goal

edit

Hi all!,

We are working with a non-holonomic robot, and with a complex mechanics to move.

We want to ignore the goal's orientation.

From ours readings and tests, the goal tolerance (xy and yaw) works only when robot arrive to goal, but when it is still far, the optimization not consider this tolerance, i.e. planner ignore that the robot can arrive to goal regardless its orientation (and it plans a long path with many loops to arrive with the specific orientation), is that right?

Can we include the tolerance in the path planning and not only consider it when the robot is arriving to the goal?

Thanks!