DDS - REP-147 Goal Interface Support for Plane (position only)

ArduPlane/AP_ExternalControl_Plane.cpp

@@ -0,0 +1,22 @@
+/*
+  external control library for plane
+ */
+
+
+#include "AP_ExternalControl_Plane.h"
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+#include "Plane.h"
+
+/*
+  Sets the target global position for a loiter point.
+*/
+bool AP_ExternalControl_Plane::set_global_position(const Location& loc)
+{
+
+    // set_target_location already checks if plane is ready for external control.
+    // It doesn't check if flying or armed, just that it's in guided mode.
+    return plane.set_target_location(loc);
+}
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

ArduPlane/AP_ExternalControl_Plane.h

@@ -0,0 +1,21 @@
+
+/*
+  external control library for plane
+ */
+#pragma once
+
+#include <AP_ExternalControl/AP_ExternalControl.h>
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+
+#include <AP_Common/Location.h>
+
+class AP_ExternalControl_Plane : public AP_ExternalControl {
+public:
+    /*
+        Sets the target global position for a loiter point.
+    */
+    bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
+};
+
+#endif // AP_EXTERNAL_CONTROL_ENABLED

ArduPlane/ArduPlane.cpp

@@ -798,8 +798,8 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
     return true;
 }
 
-#if AP_SCRIPTING_ENABLED
-// set target location (for use by scripting)
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
+// set target location (for use by external control and scripting)
 bool Plane::set_target_location(const Location &target_loc)
 {
     Location loc{target_loc};
@@ -816,7 +816,9 @@ bool Plane::set_target_location(const Location &target_loc)
     plane.set_guided_WP(loc);
     return true;
 }
+#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 
+#if AP_SCRIPTING_ENABLED
 // set target location (for use by scripting)
 bool Plane::get_target_location(Location& target_loc)
 {

ArduPlane/Plane.h

@@ -85,7 +85,7 @@
 #include <AP_Follow/AP_Follow.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 #if AP_EXTERNAL_CONTROL_ENABLED
-#include <AP_ExternalControl/AP_ExternalControl.h>
+#include "AP_ExternalControl_Plane.h"
 #endif
 
 #include "GCS_Mavlink.h"
@@ -167,6 +167,10 @@ class Plane : public AP_Vehicle {
     friend class ModeThermal;
     friend class ModeLoiterAltQLand;
 
+#if AP_EXTERNAL_CONTROL_ENABLED
+    friend class AP_ExternalControl_Plane;
+#endif
+
     Plane(void);
 
 private:
@@ -776,9 +780,9 @@ class Plane : public AP_Vehicle {
 
     AP_Param param_loader {var_info};
 
-    // dummy implementation of external control
+    // external control library
 #if AP_EXTERNAL_CONTROL_ENABLED
-    AP_ExternalControl external_control;
+    AP_ExternalControl_Plane external_control;
 #endif
 
     static const AP_Scheduler::Task scheduler_tasks[];
@@ -1241,8 +1245,10 @@ class Plane : public AP_Vehicle {
     void failsafe_check(void);
     bool is_landing() const override;
     bool is_taking_off() const override;
-#if AP_SCRIPTING_ENABLED
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
     bool set_target_location(const Location& target_loc) override;
+#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
+#if AP_SCRIPTING_ENABLED
     bool get_target_location(Location& target_loc) override;
     bool update_target_location(const Location &old_loc, const Location &new_loc) override;
     bool set_velocity_match(const Vector2f &velocity) override;

Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py

@@ -0,0 +1,213 @@
+#!/usr/bin/env python3
+# Copyright 2023 ArduPilot.org.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+"""
+Run a single-waypoint mission on Plane.
+
+Warning - This is NOT production code; it's a simple demo of capability.
+"""
+
+import math
+import rclpy
+import time
+import errno
+
+from rclpy.node import Node
+from builtin_interfaces.msg import Time
+from ardupilot_msgs.msg import GlobalPosition
+from geographic_msgs.msg import GeoPoseStamped
+from geopy import distance
+from geopy import point
+from ardupilot_msgs.srv import ArmMotors
+from ardupilot_msgs.srv import ModeSwitch
+
+
+PLANE_MODE_TAKEOFF = 13
+PLANE_MODE_GUIDED = 15
+
+FRAME_GLOBAL_INT = 5
+
+# Hard code some known locations
+# Note - Altitude in geopy is in km!
+GRAYHOUND_TRACK = point.Point(latitude=-35.345996, longitude=149.159017, altitude=0.575)
+CMAC = point.Point(latitude=-35.3627010, longitude=149.1651513, altitude=0.585)
+
+
+class PlaneWaypointFollower(Node):
+    """Plane follow waypoints using guided control."""
+
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("plane_waypoint_follower")
+
+        self.declare_parameter("arm_topic", "/ap/arm_motors")
+        self._arm_topic = self.get_parameter("arm_topic").get_parameter_value().string_value
+        self._client_arm = self.create_client(ArmMotors, self._arm_topic)
+        while not self._client_arm.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('arm service not available, waiting again...')
+
+        self.declare_parameter("mode_topic", "/ap/mode_switch")
+        self._mode_topic = self.get_parameter("mode_topic").get_parameter_value().string_value
+        self._client_mode_switch = self.create_client(ModeSwitch, self._mode_topic)
+        while not self._client_mode_switch.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('mode switch service not available, waiting again...')
+
+        self.declare_parameter("global_position_topic", "/ap/cmd_gps_pose")
+        self._global_pos_topic = self.get_parameter("global_position_topic").get_parameter_value().string_value
+        self._global_pos_pub = self.create_publisher(GlobalPosition, self._global_pos_topic, 1)
+
+        # Create subscriptions after services are up
+        self.declare_parameter("geopose_topic", "/ap/geopose/filtered")
+        self._geopose_topic = self.get_parameter("geopose_topic").get_parameter_value().string_value
+        qos = rclpy.qos.QoSProfile(
+            reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1
+        )
+
+        self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
+        self._cur_geopose = GeoPoseStamped()
+
+    def geopose_cb(self, msg: GeoPoseStamped):
+        """Process a GeoPose message."""
+        stamp = msg.header.stamp
+        if stamp.sec:
+            self.get_logger().info("From AP : Geopose [sec:{}, nsec: {}]".format(stamp.sec, stamp.nanosec))
+
+            # Store current state
+            self._cur_geopose = msg
+
+    def arm(self):
+        req = ArmMotors.Request()
+        req.arm = True
+        future = self._client_arm.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def arm_with_timeout(self, timeout: rclpy.duration.Duration):
+        """Try to arm. Returns true on success, or false if arming fails or times out."""
+        armed = False
+        start = self.get_clock().now()
+        while not armed and self.get_clock().now() - start < timeout:
+            armed = self.arm().result
+            time.sleep(1)
+        return armed
+
+    def switch_mode(self, mode):
+        req = ModeSwitch.Request()
+        assert mode in [PLANE_MODE_TAKEOFF, PLANE_MODE_GUIDED]
+        req.mode = mode
+        future = self._client_mode_switch.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration):
+        """Try to switch mode. Returns true on success, or false if mode switch fails or times out."""
+        is_in_desired_mode = False
+        start = self.get_clock().now()
+        while not is_in_desired_mode:
+            result = self.switch_mode(desired_mode)
+            # Handle successful switch or the case that the vehicle is already in expected mode
+            is_in_desired_mode = result.status or result.curr_mode == desired_mode
+            time.sleep(1)
+
+        return is_in_desired_mode
+
+    def get_cur_geopose(self):
+        """Return latest geopose."""
+        return self._cur_geopose
+
+    def send_goal_position(self, goal_global_pos):
+        """Send goal position. Must be in guided for this to work."""
+        self._global_pos_pub.publish(goal_global_pos)
+
+
+def achieved_goal(goal_global_pos, cur_geopose):
+    """Return true if the current position (LLH) is close enough to the goal (within the orbit radius)."""
+    # Use 3D geopy distance calculation
+    # https://geopy.readthedocs.io/en/stable/#module-geopy.distance
+    goal_lat = goal_global_pos
+
+    p1 = (goal_global_pos.latitude, goal_global_pos.longitude, goal_global_pos.altitude)
+    cur_pos = cur_geopose.pose.position
+    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)
+    print(f"Goal is {euclidian_distance} meters away")
+    return euclidian_distance < 150
+
+
+def main(args=None):
+    """Node entry point."""
+    rclpy.init(args=args)
+    node = PlaneWaypointFollower()
+    try:
+        # Block till armed, which will wait for EKF3 to initialize
+        if not node.arm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            raise RuntimeError("Unable to arm")
+
+        # Block till in takeoff
+        if not node.switch_mode_with_timeout(PLANE_MODE_TAKEOFF, rclpy.duration.Duration(seconds=20)):
+            raise RuntimeError("Unable to switch to takeoff mode")
+
+        is_ascending_to_takeoff_alt = True
+        while is_ascending_to_takeoff_alt:
+            rclpy.spin_once(node)
+            time.sleep(1.0)
+
+            # Hard code waiting in takeoff to reach operating altitude of 630m
+            # This is just a hack because geopose is reported with absolute rather than relative altitude,
+            # and this node doesn't have access to the terrain data
+            is_ascending_to_takeoff_alt = node.get_cur_geopose().pose.position.altitude < CMAC.altitude * 1000 + 45
+
+        if is_ascending_to_takeoff_alt:
+            raise RuntimeError("Failed to reach takeoff altitude")
+
+        if not node.switch_mode_with_timeout(PLANE_MODE_GUIDED, rclpy.duration.Duration(seconds=20)):
+            raise RuntimeError("Unable to switch to guided mode")
+
+        # Send a guided WP with location, frame ID, alt frame
+        goal_pos = GlobalPosition()
+        goal_pos.latitude = GRAYHOUND_TRACK.latitude
+        goal_pos.longitude = GRAYHOUND_TRACK.longitude
+        DESIRED_AGL = 60
+        goal_pos.altitude = GRAYHOUND_TRACK.altitude * 1000 + DESIRED_AGL
+        goal_pos.coordinate_frame = FRAME_GLOBAL_INT
+        goal_pos.header.frame_id = "map"
+
+        node.send_goal_position(goal_pos)
+
+        start = node.get_clock().now()
+        has_achieved_goal = False
+        while not has_achieved_goal and node.get_clock().now() - start < rclpy.duration.Duration(seconds=120):
+            rclpy.spin_once(node)
+            has_achieved_goal = achieved_goal(goal_pos, node.get_cur_geopose())
+            time.sleep(1.0)
+
+        if not has_achieved_goal:
+            raise RuntimeError("Unable to achieve goal location")
+
+        print("Goal achieved")
+
+    except KeyboardInterrupt:
+        pass
+
+    # Destroy the node explicitly.
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

Tools/ros2/ardupilot_dds_tests/package.xml

@@ -8,11 +8,13 @@
   <license>GLP-3.0</license>
 
   <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>ardupilot_msgs</exec_depend>
   <exec_depend>ardupilot_sitl</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_pytest</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>micro_ros_msgs</exec_depend>
+  <exec_depend>python3-geopy</exec_depend>
   <exec_depend>python3-pytest</exec_depend>
   <exec_depend>rclpy</exec_depend>
   <exec_depend>socat</exec_depend>
@@ -23,6 +25,7 @@
   <test_depend>ament_uncrustify</test_depend>
   <test_depend>ament_xmllint</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ardupilot_msgs</test_depend>
   <test_depend>ardupilot_sitl</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_pytest</test_depend>

Tools/ros2/ardupilot_dds_tests/setup.py

@@ -25,6 +25,7 @@
     entry_points={
         'console_scripts': [
             "time_listener = ardupilot_dds_tests.time_listener:main",
+            "plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main",
         ],
     },
 )

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
 )