search

ros-planning / navigation

ROS Navigation stack. Code for finding where the robot is and how it can get somewhere else.
2.35k stars 1.8k forks source link

Navnf capability on planning affected by the dimensions of the occupancy grid #1235

Open samuelpg opened 1 year ago

samuelpg commented 1 year ago

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.

planner_not_working

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.

planner_working

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

mikeferguson commented 1 year ago

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.

SteveMacenski commented 1 year ago

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

mikeferguson commented 1 year ago

I'm really surprised bugs like this crop up in a codebase that is almost 15 years old....