From 6195046fe84249ca0afcfbf86a1bbd4f350881c7 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 6 Dec 2023 23:49:01 -0700 Subject: [PATCH 1/7] AP_DDS: add REP-147 Global Position Control Signed-off-by: Ryan Friedman --- libraries/AP_DDS/AP_DDS_Client.cpp | 16 +++++ libraries/AP_DDS/AP_DDS_Client.h | 4 ++ libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 63 +++++++++++++++++++ libraries/AP_DDS/AP_DDS_ExternalControl.h | 8 +++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 11 ++++ .../Idl/ardupilot_msgs/msg/GlobalPosition.idl | 60 ++++++++++++++++++ libraries/AP_DDS/README.md | 5 ++ libraries/AP_DDS/dds_xrce_profile.xml | 15 +++++ 8 files changed, 182 insertions(+) create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 43dd37f788709..b743ed8aa0151 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -44,6 +44,7 @@ static constexpr uint16_t DELAY_PING_MS = 500; sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {}; tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {}; geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {}; +ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {}; const AP_Param::GroupInfo AP_DDS_Client::var_info[] { @@ -391,6 +392,8 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) if (ahrs.get_location(loc)) { msg.pose.position.latitude = loc.lat * 1E-7; msg.pose.position.longitude = loc.lng * 1E-7; + // TODO this is assumed to be absolute frame in WGS-84 as per the GeoPose message definition in ROS. + // Use loc.get_alt_frame() to convert if necessary. msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m } @@ -502,6 +505,19 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) { // TODO #23430 handle velocity control failure through rosout, throttled. } +#endif // AP_EXTERNAL_CONTROL_ENABLED + break; + } + case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: { + const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic); + if (success == false) { + break; + } + +#if AP_EXTERNAL_CONTROL_ENABLED + if (!AP_DDS_External_Control::handle_global_position_control(rx_global_position_control_topic)) { + // TODO #23430 handle global position control failure through rosout, throttled. + } #endif // AP_EXTERNAL_CONTROL_ENABLED break; } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 19d20c3017d24..3751a697e8c1b 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -4,6 +4,8 @@ #include "uxr/client/client.h" #include "ucdr/microcdr.h" + +#include "ardupilot_msgs/msg/GlobalPosition.h" #include "builtin_interfaces/msg/Time.h" #include "sensor_msgs/msg/NavSatFix.h" @@ -65,6 +67,8 @@ class AP_DDS_Client static sensor_msgs_msg_Joy rx_joy_topic; // incoming REP147 velocity control static geometry_msgs_msg_TwistStamped rx_velocity_control_topic; + // incoming REP147 goal interface global position + static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic; // outgoing transforms tf2_msgs_msg_TFMessage tx_static_transforms_topic; tf2_msgs_msg_TFMessage tx_dynamic_transforms_topic; diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index a72536d6ffde8..fcc6fc5d306a1 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -6,6 +6,49 @@ #include +// These are the Goal Interface constants. Because microxrceddsgen does not expose +// them in the generated code, they are manually maintained +// Position ignore flags +static constexpr uint16_t TYPE_MASK_IGNORE_LATITUDE = 1; +static constexpr uint16_t TYPE_MASK_IGNORE_LONGITUDE = 2; +static constexpr uint16_t TYPE_MASK_IGNORE_ALTITUDE = 4; + +bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + if (strcmp(cmd_pos.header.frame_id, MAP_FRAME) == 0) { + // Narrow the altitude + const int32_t alt_cm = static_cast(cmd_pos.altitude * 100); + + Location::AltFrame alt_frame; + if (!convert_alt_frame(cmd_pos.coordinate_frame, alt_frame)) { + return false; + } + + constexpr uint32_t MASK_POS_IGNORE = + TYPE_MASK_IGNORE_LATITUDE | + TYPE_MASK_IGNORE_LONGITUDE | + TYPE_MASK_IGNORE_ALTITUDE; + + if (!(cmd_pos.type_mask & MASK_POS_IGNORE)) { + Location loc(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame); + if (!external_control->set_global_position(loc)) { + return false; // Don't try sending other commands if this fails + } + } + + // TODO add velocity and accel handling + + return true; + } + + return false; +} + bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel) { auto *external_control = AP::externalcontrol(); @@ -40,5 +83,25 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta return false; } +bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out) +{ + + // Specified in ROS REP-147; only some are supported. + switch (frame_in) { + case 5: // FRAME_GLOBAL_INT + frame_out = Location::AltFrame::ABSOLUTE; + break; + case 6: // FRAME_GLOBAL_REL_ALT + frame_out = Location::AltFrame::ABOVE_HOME; + break; + case 11: // FRAME_GLOBAL_TERRAIN_ALT + frame_out = Location::AltFrame::ABOVE_TERRAIN; + break; + default: + return false; + } + return true; +} + #endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h index dbffafdd81ad1..3986ef63774aa 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.h +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.h @@ -1,11 +1,19 @@ #pragma once #if AP_DDS_ENABLED +#include "ardupilot_msgs/msg/GlobalPosition.h" #include "geometry_msgs/msg/TwistStamped.h" +#include + class AP_DDS_External_Control { public: + // REP-147 Goal Interface Global Position Control + // https://ros.org/reps/rep-0147.html#goal-interface + static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos); static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel); +private: + static bool convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out); }; #endif // AP_DDS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 8ce9507d08005..8df056d484fc7 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -22,6 +22,7 @@ enum class TopicIndex: uint8_t { JOY_SUB, DYNAMIC_TRANSFORMS_SUB, VELOCITY_CONTROL_SUB, + GLOBAL_POSITION_SUB, }; static inline constexpr uint8_t to_underlying(const TopicIndex index) @@ -142,4 +143,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_profile_label = "", .dr_profile_label = "velocitycontrol__dr", }, + { + .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), + .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), + .sub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAREADER_ID}, + .topic_profile_label = "globalposcontrol__t", + .dw_profile_label = "", + .dr_profile_label = "globalposcontrol__dr", + }, }; diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl new file mode 100644 index 0000000000000..7b6325d9e0201 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl @@ -0,0 +1,60 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/GlobalPosition.msg +// generated code does not contain a copyright notice + +#include "geometry_msgs/msg/Twist.idl" +#include "std_msgs/msg/Header.idl" + +module ardupilot_msgs { + module msg { + module GlobalPosition_Constants { + const uint8 FRAME_GLOBAL_INT = 5; + const uint8 FRAME_GLOBAL_REL_ALT = 6; + const uint8 FRAME_GLOBAL_TERRAIN_ALT = 11; + @verbatim (language="comment", text= + "Position ignore flags") + const uint16 IGNORE_LATITUDE = 1; + const uint16 IGNORE_LONGITUDE = 2; + const uint16 IGNORE_ALTITUDE = 4; + @verbatim (language="comment", text= + "Velocity vector ignore flags") + const uint16 IGNORE_VX = 8; + const uint16 IGNORE_VY = 16; + const uint16 IGNORE_VZ = 32; + @verbatim (language="comment", text= + "Acceleration/Force vector ignore flags") + const uint16 IGNORE_AFX = 64; + const uint16 IGNORE_AFY = 128; + const uint16 IGNORE_AFZ = 256; + @verbatim (language="comment", text= + "Force in af vector flag") + const uint16 FORCE = 512; + const uint16 IGNORE_YAW = 1024; + const uint16 IGNORE_YAW_RATE = 2048; + }; + @verbatim (language="comment", text= + "Experimental REP-147 Goal Interface" "\n" + "https://ros.org/reps/rep-0147.html#goal-interface") + struct GlobalPosition { + std_msgs::msg::Header header; + + uint8 coordinate_frame; + + uint16 type_mask; + + double latitude; + + double longitude; + + @verbatim (language="comment", text= + "in meters, AMSL or above terrain") + float altitude; + + geometry_msgs::msg::Twist velocity; + + geometry_msgs::msg::Twist acceleration_or_force; + + float yaw; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 1af0af6f6001a..d8b0b088bdf0b 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -216,6 +216,7 @@ Published topics: * /rosout [rcl_interfaces/msg/Log] 1 publisher Subscribed topics: + * /ap/cmd_gps_pose [ardupilot_msgs/msg/GlobalPosition] 1 subscriber * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber @@ -311,6 +312,10 @@ cp /opt/ros/humble/share/builtin_interfaces/msg/Time.idl libraries/AP_DDS/Idl/bu # Build the code again with the `--enable-dds` flag as described above ``` +If the message is custom for ardupilot, first create the ROS message in `Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg`. +Then, build ardupilot_msgs with colcon. +Finally, copy the IDL folder from the install directory into the source tree. + ### Rules for adding topics and services to `dds_xrce_profile.xml` Topics and services available from `AP_DDS` are automatically mapped into ROS 2 diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index abfa47779e9ab..b8e80c6e49e6c 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -276,4 +276,19 @@ rq/ap/mode_switchRequest rr/ap/mode_switchReply + + rt/ap/cmd_gps_pose + ardupilot_msgs::msg::dds_::GlobalPosition_ + + KEEP_LAST + 5 + + + + + NO_KEY + rt/ap/cmd_gps_pose + ardupilot_msgs::msg::dds_::GlobalPosition_ + + From dcacee4a4ac1d3d80b5148d1b98c93bd4ac2e9b2 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 6 Dec 2023 23:49:02 -0700 Subject: [PATCH 2/7] AP_ExternalControl: add REP-147 Global Position Control Signed-off-by: Ryan Friedman --- ArduPlane/AP_ExternalControl_Plane.cpp | 22 +++++++++++++++++++ .../AP_ExternalControl/AP_ExternalControl.h | 10 +++++++++ 2 files changed, 32 insertions(+) create mode 100644 ArduPlane/AP_ExternalControl_Plane.cpp diff --git a/ArduPlane/AP_ExternalControl_Plane.cpp b/ArduPlane/AP_ExternalControl_Plane.cpp new file mode 100644 index 0000000000000..c5afabf584321 --- /dev/null +++ b/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 diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index b72190d9c4da8..34228e2b7ab4f 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -8,6 +8,7 @@ #if AP_EXTERNAL_CONTROL_ENABLED +#include #include class AP_ExternalControl @@ -24,9 +25,18 @@ class AP_ExternalControl return false; } + /* + Sets the target global position with standard guided mode behavior. + */ + virtual bool set_global_position(const Location& loc) WARN_IF_UNUSED { + return false; + } + static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED { return singleton; } +protected: + ~AP_ExternalControl() {} private: static AP_ExternalControl *singleton; From 1edd86daaba5690c5807e53c4e0e23254b4cd975 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 6 Dec 2023 23:49:02 -0700 Subject: [PATCH 3/7] ArduPlane: add REP-147 Global Position Control Signed-off-by: Ryan Friedman --- ArduPlane/AP_ExternalControl_Plane.h | 21 +++++++++++++++++++++ ArduPlane/Plane.h | 10 +++++++--- 2 files changed, 28 insertions(+), 3 deletions(-) create mode 100644 ArduPlane/AP_ExternalControl_Plane.h diff --git a/ArduPlane/AP_ExternalControl_Plane.h b/ArduPlane/AP_ExternalControl_Plane.h new file mode 100644 index 0000000000000..71308780d55fd --- /dev/null +++ b/ArduPlane/AP_ExternalControl_Plane.h @@ -0,0 +1,21 @@ + +/* + external control library for plane + */ +#pragma once + +#include + +#if AP_EXTERNAL_CONTROL_ENABLED + +#include + +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 diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 80e3cb8636ce1..66852d99d4d75 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -85,7 +85,7 @@ #include #include #if AP_EXTERNAL_CONTROL_ENABLED -#include +#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[]; From 4518bace4019bd05c23cedfeef2100f261c1b326 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 6 Dec 2023 23:49:02 -0700 Subject: [PATCH 4/7] Tools: add REP-147 Global Position Control Signed-off-by: Ryan Friedman --- .../plane_waypoint_follower.py | 213 ++++++++++++++++++ Tools/ros2/ardupilot_dds_tests/package.xml | 3 + Tools/ros2/ardupilot_dds_tests/setup.py | 1 + Tools/ros2/ardupilot_msgs/CMakeLists.txt | 4 + .../ardupilot_msgs/msg/GlobalPosition.msg | 30 +++ Tools/ros2/ardupilot_msgs/package.xml | 3 + 6 files changed, 254 insertions(+) create mode 100755 Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py create mode 100644 Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py new file mode 100755 index 0000000000000..27de6f272f2ed --- /dev/null +++ b/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 . + +""" +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() diff --git a/Tools/ros2/ardupilot_dds_tests/package.xml b/Tools/ros2/ardupilot_dds_tests/package.xml index 7b44fbffdcbf9..b4cc9fa2a0e35 100644 --- a/Tools/ros2/ardupilot_dds_tests/package.xml +++ b/Tools/ros2/ardupilot_dds_tests/package.xml @@ -8,11 +8,13 @@ GLP-3.0 ament_index_python + ardupilot_msgs ardupilot_sitl launch launch_pytest launch_ros micro_ros_msgs + python3-geopy python3-pytest rclpy socat @@ -23,6 +25,7 @@ ament_uncrustify ament_xmllint ament_lint_auto + ardupilot_msgs ardupilot_sitl launch launch_pytest diff --git a/Tools/ros2/ardupilot_dds_tests/setup.py b/Tools/ros2/ardupilot_dds_tests/setup.py index 3d2039beb5c0f..bbe11e6a5c812 100644 --- a/Tools/ros2/ardupilot_dds_tests/setup.py +++ b/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", ], }, ) diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index 8096431370687..d066a2d32b2e4 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 0000000000000..f34b84c3ea34e --- /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 08e92bf1df7d9..2e2a118fa5ae2 100644 --- a/Tools/ros2/ardupilot_msgs/package.xml +++ b/Tools/ros2/ardupilot_msgs/package.xml @@ -11,6 +11,9 @@ ament_cmake rosidl_default_generators + std_msgs + geometry_msgs + rosidl_default_runtime ament_cmake_copyright From 090453de2013dd4d48358eccf639c24d88515aa6 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 7 Dec 2023 00:10:04 -0700 Subject: [PATCH 5/7] AP_Common: Define units for locatoin data members Signed-off-by: Ryan Friedman --- libraries/AP_Common/Location.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 134a27e590af3..eab834e2d427f 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -15,9 +15,9 @@ class Location uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location // note that mission storage only stores 24 bits of altitude (~ +/- 83km) - int32_t alt; - int32_t lat; - int32_t lng; + int32_t alt; // in cm + int32_t lat; // in 1E7 degrees + int32_t lng; // in 1E7 degrees /// enumeration of possible altitude types enum class AltFrame { From b6e5a2032aca20f245f08c6c69127499c7c16297 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 12 Dec 2023 21:31:29 -0700 Subject: [PATCH 6/7] AP_Vehicle: Set position target depends on ext control * Used to depend on scripting but now it's used in AP_ExternalControl Signed-off-by: Ryan Friedman --- libraries/AP_Vehicle/AP_Vehicle.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 1b12f3505c312..5ff1dca6c03f2 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include // Notify library @@ -151,12 +152,15 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // returns true if the vehicle has crashed virtual bool is_crashed() const; +#if AP_EXTERNAL_CONTROL_ENABLED + // Method to control vehicle position for use by external control + virtual bool set_target_location(const Location& target_loc) { return false; } +#endif // AP_EXTERNAL_CONTROL_ENABLED #if AP_SCRIPTING_ENABLED /* methods to control vehicle for use by scripting */ virtual bool start_takeoff(float alt) { return false; } - virtual bool set_target_location(const Location& target_loc) { return false; } virtual bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) { return false; } virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; } virtual bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; } From 19a1ff37d24be6d0cb4412917c711df1b7b01cf3 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 12 Dec 2023 21:32:43 -0700 Subject: [PATCH 7/7] ArduPlane: Make set position tgt depend on ext ctrl * Set position target used to just be used in scripting, now it's used by DDS in external control Signed-off-by: Ryan Friedman --- ArduPlane/ArduPlane.cpp | 6 ++++-- ArduPlane/Plane.h | 4 +++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 36bc4f5b53e09..02dcd401eb2dd 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 66852d99d4d75..71936b61a4e8f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1245,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;