Add null check for finalPathPoint. Fixes #168