mirror of https://github.com/ArduPilot/ardupilot
Tools: ros2: Reformat
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
parent
d6cfc392a2
commit
c16fee3f47
|
@ -144,7 +144,7 @@ def achieved_goal(goal_global_pos, cur_geopose):
|
|||
p2 = (cur_pos.latitude, cur_pos.longitude, cur_pos.altitude)
|
||||
|
||||
flat_distance = distance.distance(p1[:2], p2[:2]).m
|
||||
euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2)
|
||||
euclidian_distance = math.sqrt(flat_distance ** 2 + (p2[2] - p1[2]) ** 2)
|
||||
print(f"Goal is {euclidian_distance} meters away")
|
||||
return euclidian_distance < 150
|
||||
|
||||
|
|
|
@ -35,6 +35,7 @@ TRUE_STRING = "True"
|
|||
FALSE_STRING = "False"
|
||||
BOOL_STRING_CHOICES = set([TRUE_STRING, FALSE_STRING])
|
||||
|
||||
|
||||
class VirtualPortsLaunch:
|
||||
"""Launch functions for creating virtual ports using `socat`."""
|
||||
|
||||
|
@ -369,16 +370,10 @@ class MAVProxyLaunch:
|
|||
description="SITL output port.",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"map",
|
||||
default_value="False",
|
||||
description="Enable MAVProxy Map.",
|
||||
choices=BOOL_STRING_CHOICES
|
||||
"map", default_value="False", description="Enable MAVProxy Map.", choices=BOOL_STRING_CHOICES
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"console",
|
||||
default_value="False",
|
||||
description="Enable MAVProxy Console.",
|
||||
choices=BOOL_STRING_CHOICES
|
||||
"console", default_value="False", description="Enable MAVProxy Console.", choices=BOOL_STRING_CHOICES
|
||||
),
|
||||
]
|
||||
|
||||
|
|
Loading…
Reference in New Issue