From 417b938b8a075fd9cda8c7e727caea7d89b08bbc Mon Sep 17 00:00:00 2001
From: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Date: Wed, 6 Dec 2023 23:49:02 -0700
Subject: [PATCH] Tools: add REP-147 Global Position Control

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
---
 Tools/ros2/ardupilot_msgs/CMakeLists.txt      |  4 +++
 .../ardupilot_msgs/msg/GlobalPosition.msg     | 30 +++++++++++++++++++
 Tools/ros2/ardupilot_msgs/package.xml         |  3 ++
 3 files changed, 37 insertions(+)
 create mode 100644 Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg

diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt
index 8096431370687c..d066a2d32b2e43 100644
--- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt
+++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt
@@ -5,14 +5,18 @@ project(ardupilot_msgs)
 # Find dependencies.
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 # --------------------------------------------------------------------------- #
 # Generate and export message interfaces.
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/GlobalPosition.msg"
   "srv/ArmMotors.srv"
   "srv/ModeSwitch.srv"
+  DEPENDENCIES geometry_msgs std_msgs
   ADD_LINTER_TESTS
 )
 
diff --git a/Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg b/Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg
new file mode 100644
index 00000000000000..f34b84c3ea34e8
--- /dev/null
+++ b/Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg
@@ -0,0 +1,30 @@
+# Experimental REP-147 Goal Interface
+# https://ros.org/reps/rep-0147.html#goal-interface
+
+std_msgs/Header header
+
+uint8 coordinate_frame
+uint8 FRAME_GLOBAL_INT = 5
+uint8 FRAME_GLOBAL_REL_ALT = 6
+uint8 FRAME_GLOBAL_TERRAIN_ALT = 11
+
+uint16 type_mask
+uint16 IGNORE_LATITUDE = 1 # Position ignore flags
+uint16 IGNORE_LONGITUDE = 2
+uint16 IGNORE_ALTITUDE = 4
+uint16 IGNORE_VX = 8  # Velocity vector ignore flags
+uint16 IGNORE_VY = 16
+uint16 IGNORE_VZ = 32
+uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags
+uint16 IGNORE_AFY = 128
+uint16 IGNORE_AFZ = 256
+uint16 FORCE = 512 # Force in af vector flag
+uint16 IGNORE_YAW = 1024
+uint16 IGNORE_YAW_RATE = 2048
+
+float64 latitude
+float64 longitude
+float32 altitude # in meters, AMSL or above terrain
+geometry_msgs/Twist velocity
+geometry_msgs/Twist acceleration_or_force
+float32 yaw
diff --git a/Tools/ros2/ardupilot_msgs/package.xml b/Tools/ros2/ardupilot_msgs/package.xml
index 08e92bf1df7d99..2e2a118fa5ae2c 100644
--- a/Tools/ros2/ardupilot_msgs/package.xml
+++ b/Tools/ros2/ardupilot_msgs/package.xml
@@ -11,6 +11,9 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_cmake_copyright</test_depend>