Tools: ros2: Use 3D distance in goal computation

Signed-off-by: Ryan Friedman <[email protected]>

Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py

@@ -19,6 +19,7 @@
 Warning - This is NOT production code; it's a simple demo of capability.
 """
 
+import math
 import rclpy
 import time
 import errno
@@ -144,9 +145,10 @@ def achieved_goal(goal_global_pos, cur_geopose):
     cur_pos = cur_geopose.pose.position
     p2 = (cur_pos.latitude, cur_pos.longitude, cur_pos.altitude)
 
-    flat_distance_km = distance.distance(p1[:2], p2[:2])
-    print(f"Goal is {flat_distance_km} away")
-    return flat_distance_km < 0.15
+    flat_distance = distance.distance(p1[:2], p2[:2]).m
+    euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2)
+    print(f"Goal is {euclidian_distance} meters away")
+    return euclidian_distance < 150
 
 
 def main(args=None):