[smac_planner] On Hybrid A*, any reason to use floor to quantize angles?

[corot]

This is a question (with a potential follow up PR) so someone please tag it accordingly.

Hybrid A* operates with quantizied angles (72 bins by default) so it converts all angles (including start and goal poses) to a bin. It does that by dividing the angles by angle_bin_size (2pi / 72) and getting the integer part using floor. This introduces quantization errors of up to 0.087266463 radians, which can be a problem in some cases, e.g if the goal yaw tolerance is smaller than that number.

On this PR over our backport of smac_planner to ROS 1, I'm replacing floor with round, halving the worst case error to 0,043633231. I can open a similar PR to this repo, if the maintainers consider it a positive change.

[SteveMacenski]

Mostly to handle the situation that we don't overflow and because the motion primitives are exact increments of angular bins, so there's no chance that during the planning that we have to round the angles, they are exactly searched.

Note that anywhere that you do something like orientation_bin = round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size); --> you need to make sure that orientation_bin is checked for an overflow, the same way you do in return bin < num_angle_quantization ? bin : 0;

A port back here would be good I think! Just make sure to update the lattice planner with the same changes

[SteveMacenski]

@corot https://github.com/ros-navigation/navigation2/pull/4636 implements for Nav2, I'd appreciate a review, but its a minor riff on yours

[corot]

@corot #4636 implements for Nav2, I'd appreciate a review, but its a minor riff on yours

Ah,,, thanks and sorry for not doing the PR myself

[SteveMacenski]

@corot I think I found a potential issue alongside this one that wasn't solved. See https://github.com/ros-navigation/navigation2/blob/3233b39eeb20bf6fe5503dc76dc48d23a67cea85/nav2_smac_planner/src/analytic_expansion.cpp#L225

This will implicitly floor from the unsigned int casting. Should we round and check the % bin_size when converting into the index?

[corot]

This will implicitly floor from the unsigned int casting. Should we round and check the % bin_size when converting into the index?

@SteveMacenski, I think it's fine. angle is already a bin, so it should be always be an integer (I made a fast check and indeed it is)