Shortest path finder suggesting a path out of the mesh

[emilia-szymanska]

Hi hi! I have a navigation mesh and two points between which I want to find the shortest path considering the navigable areas. For most of the cases it works great (especially if the navmesh is rather flat), however I started experiencing issues as in the following figure:

From top view everything looks alright, but after further check you can see that the shortest path is not fully contained in the navmesh (it is 0.5 meter away from the start point). Snapping works correctly, and I pass the snapped points to the path finder to finally retrieve the waypoints forming the shortest path (and I plot it on the same plot as navmesh visualization):

pt = habitat_sim.PathFinder()
pt.load_nav_mesh(navmesh_filename)
shortest_path = habitat_sim.ShortestPath()
shortest_path.requested_end = pt.snap_point(end)
shortest_path.requested_start = pt.snap_point(start)
if pt.find_path(shortest_path):
    print(f"Success! Distance: {shortest_path.geodesic_distance}")
    return shortest_path.points

I would be grateful for any hints on what I am doing wrong or whether it is a known limitation of habitat sim.

Habitat-sim version 0.3.1 Ubuntu 22.04

[aclegg3]

Hey @emilia-szymanska,

I see what you mean here. I'm curious, however, if this is a bug in the pathfinder or something related to other code such as the visualization.

It would be most helpful if you had something reproducible or more log information to work off of.

It is also the case that a two point path may be the result of a pathfinding failure. I see you are checking the return of find_path, but is that check a pre-condition to plotting and other logic?

Happy to help if there really is a bug and we can figure out how to reproduce it.

[emilia-szymanska]

Hi @aclegg3, since it's been already 3 weeks, I am not sure which scene I was referring to when I created the issue, so I reproduced the problem on another one:

import habitat_sim
import numpy as np

shortest_path = habitat_sim.ShortestPath()
pathfinder = habitat_sim.PathFinder()
pathfinder.load_nav_mesh("mesh_semantic.navmesh")

start = np.array([-0.667, -0.804,  0.473])
goal = np.array([-1.893, 0.099, 1.966])

snapped_start = pathfinder.snap_point(start)
snapped_goal = pathfinder.snap_point(goal)

shortest_path.requested_end = snapped_goal
shortest_path.requested_start = snapped_start
if pathfinder.find_path(shortest_path):
    dist = shortest_path.geodesic_distance
    print(shortest_path.points)

Link to the navmesh I used, uploaded to Google Drive (let me know if you have problems accessing it): https://drive.google.com/file/d/1A4z-lVJB7WCcVgkgjvMDD958q7r-e_VW/view?usp=sharing

After running habitat-sim, I get the following shortest path:

[array([-0.667     , -0.42650425,  0.473     ], dtype=float32), array([-1.4271741 , -0.22650433,  1.4520178 ], dtype=float32), array([-1.5771741, -0.4265043,  1.4520178], dtype=float32), array([-1.8270283 , -0.33564818,  1.4217324 ], dtype=float32)]

Here you can see that the shortest path is not on the navmesh:

[emilia-szymanska]

@aclegg3 for reference, I used pygeodesic library and I got a path that is within the mesh as depicted here:

The snapping is different, because I implemented it in some other way than habitat sim, but at least the shortest path is contained within the mesh, and I use the same visualization script as for the habitat-sim, so I believe the visualization is correct.