Fixed a small bug where the getClosestWaypoint function would start at index 1 instead of 0, which caused it to miss the first point which could actually be the closest point.
Also removed logging from library functions, it should be up to the caller of the function to check return values and produce appropriate log messages according to the use case.
Fixed a small bug where the getClosestWaypoint function would start at index 1 instead of 0, which caused it to miss the first point which could actually be the closest point.
Also removed logging from library functions, it should be up to the caller of the function to check return values and produce appropriate log messages according to the use case.