ros2: update terrain_navigation_ros

- Add Python node to relay from /mavros/set_point/global to /ap/cmd_gps_pose.

Signed-off-by: Rhys Mainwaring <[email protected]>

terrain_navigation_ros/scripts/global_postion_relay.py

@@ -0,0 +1,64 @@
+"""
+ROS 2 node to convert from mavros_msgs/GlobalPositionTarget Message
+to ardupilot_msgs/GlobalPosition
+"""
+
+import sys
+
+import rclpy
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+
+from mavros_msgs.msg import GlobalPositionTarget
+from ardupilot_msgs.msg import GlobalPosition
+
+
+class GlobalPositionRelay(Node):
+    def __init__(self):
+        super().__init__("global_position_relay")
+
+        self.pub = self.create_publisher(
+            GlobalPosition,
+            "/ap/cmd_gps_pose",
+            10,
+        )
+
+        self.sub = self.create_subscription(
+            GlobalPositionTarget,
+            "/mavros/setpoint_raw/global",
+            self.global_position_cb,
+            10,
+        )
+
+    def global_position_cb(self, msg):
+
+        pos_msg = GlobalPosition()
+        pos_msg.header = msg.header
+        pos_msg.header.frame_id = "map"
+        pos_msg.latitude = msg.latitude
+        pos_msg.longitude = msg.longitude
+        pos_msg.altitude = msg.altitude
+
+        self.pub.publish(pos_msg)
+
+        self.get_logger().info("recv: {}\n".format(msg))
+        self.get_logger().info("send: {}\n".format(pos_msg))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = GlobalPositionRelay()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    except ExternalShutdownException:
+        sys.exit(1)
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()