Navnf capability on planning affected by the dimensions of the occupancy grid
samuelpg opened this issue · comments
We discovered some interesting behavior of the navnf planner while testing the navigation stack in a very long hallway, pictured bellow, where the outcome of the plan depended on the distance to the goal sent to the robot.
If the distance to the goal from the current pose of the robot was less than around 30 meters, the planner calculated a path and the robot was able to navigate to the goal as expected. If the distance to the goal was greater than 30 meters, then the planner would calculate a path of about 25 meters that started at the goal and moved towards the robot; but the path would not reach the robot and as a result the robot considered the plan as empty. This is shown in the image above, where the path calculated is in green. The complete log file is available here: rosout.log
From other simulated environments, we knew that the configuration of the robot and the navigation stack was correct, as we did not have this kind of issues even when the goal was over 120 meters away. We tried to change multiple parameters of the planners, but this did not remove the strange behavior.
Investigating further through the source code we found these two lines:
https://github.com/ros-planning/navigation/blob/noetic-devel/navfn/src/navfn_ros.cpp#L402
https://github.com/ros-planning/navigation/blob/noetic-devel/navfn/include/navfn/navfn.h#L247
Our interpretation is that the plan calculation depends on the number of cells in the x axis only. Our original hallway is aligned to the y axis (as pictured above, notice the orientation of the map frame), so the map dimensions in y are much larger than the dimensions in x (the dimensions are 250 X 2666 to be exact). We believe this is the reason the planner was not able to calculate a path that reached the robot when the goal was at the other side of the hallway. To confirm this, we modified the hallway so that it aligned to the x axis (pictured below). The robot was able to find a path from one side to the other, as expected.
We believe that this is a bug, as we could not find a mention in the documentation regarding constraints in the dimensions of the occupancy grid map, such that the dimensions in x must be greater than the dimension in y.
Here are the occupancy grid maps for the two scenarios described above: hallway_x_and_y.zip
Yup - this clearly looks like a bug - easiest fix is probably to change that to max(x_size, y_size) - if you want to open a PR for that, we'd definitely accept/merge.
Interestingly, we made this change some years ago in Nav2 as well https://github.com/ros-planning/navigation2/blob/b8bd93fa658ef37bf25c64df478cc3b0cec0c608/nav2_navfn_planner/src/navfn_planner.cpp#L399-L402
I'm really surprised bugs like this crop up in a codebase that is almost 15 years old....