From c742f5670ec67a725a76cff9381f6397c45c0ae6 Mon Sep 17 00:00:00 2001 From: Sanket Sharma Date: Mon, 29 Apr 2024 16:01:10 +0530 Subject: [PATCH] AP_DDS: Added copter takeoff support --- ArduCopter/Copter.cpp | 4 +- ArduCopter/Copter.h | 2 +- ArduCopter/mode.cpp | 1 + .../ardupilot_dds_tests/copter_takeoff.py | 180 ++++++++++++++++++ Tools/ros2/ardupilot_dds_tests/setup.py | 1 + Tools/ros2/ardupilot_msgs/CMakeLists.txt | 1 + Tools/ros2/ardupilot_msgs/srv/Takeoff.srv | 10 + libraries/AP_DDS/AP_DDS_Client.cpp | 28 +++ libraries/AP_DDS/AP_DDS_Service_Table.h | 9 +- .../AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl | 20 ++ libraries/AP_DDS/README.md | 12 ++ libraries/AP_DDS/dds_xrce_profile.xml | 4 + 12 files changed, 269 insertions(+), 3 deletions(-) create mode 100755 Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py create mode 100644 Tools/ros2/ardupilot_msgs/srv/Takeoff.srv create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a16286a6a4a936..e0cda18e3daac5 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -272,7 +272,8 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Copter::_failsafe_priorities[7]; -#if AP_SCRIPTING_ENABLED + +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED == ENABLED // start takeoff to given altitude (for use by scripting) bool Copter::start_takeoff(float alt) @@ -289,6 +290,7 @@ bool Copter::start_takeoff(float alt) return false; } + // set target location (for use by scripting) bool Copter::set_target_location(const Location& target_loc) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3285f267976e5c..98ce438ae2f0ff 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -666,7 +666,7 @@ class Copter : public AP_Vehicle { void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override; -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED == ENABLED bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index fe900d307db9ae..51097c93074fd9 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -248,6 +248,7 @@ bool Copter::gcs_mode_enabled(const Mode::Number mode_num) // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was successfully set // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately + bool Copter::set_mode(Mode::Number mode, ModeReason reason) { // update last reason diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py new file mode 100755 index 00000000000000..10f3c43426496c --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py @@ -0,0 +1,180 @@ +#!/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 takeoff test on Copter. + +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 +from ardupilot_msgs.srv import Takeoff + + + +COPTER_MODE_GUIDED = 4 + +TAKEOFF_ALT = 10 + +class CopterTakeoff(Node): + """Copter takeoff using guided control.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("copter_takeoff") + + 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("takeoff_service", "/ap/takeoff") + self._takeoff_topic = self.get_parameter("takeoff_service").get_parameter_value().string_value + self._client_takeoff = self.create_client(Takeoff, self._takeoff_topic) + while not self._client_takeoff.wait_for_service(timeout_sec=1.0): + self.get_logger().info('takeoff service not available, waiting again...') + + 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 [COPTER_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 takeoff(self, alt): + req = Takeoff.Request() + req.alt = alt + future = self._client_takeoff.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result() + + def takeoff_with_timeout(self, alt: int, timeout: rclpy.duration.Duration): + """Try to takeoff. Returns true on success, or false if takeoff fails or times out.""" + takeoff_success = False + start = self.get_clock().now() + while not takeoff_success: + result = self.takeoff(alt) + takeoff_success = result.status + time.sleep(1) + + return takeoff_success + + def get_cur_geopose(self): + """Return latest geopose.""" + return self._cur_geopose + +def main(args=None): + """Node entry point.""" + rclpy.init(args=args) + node = CopterTakeoff() + try: + if not node.switch_mode_with_timeout(COPTER_MODE_GUIDED, rclpy.duration.Duration(seconds=20)): + raise RuntimeError("Unable to switch to guided mode") + # 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.takeoff_with_timeout(TAKEOFF_ALT, rclpy.duration.Duration(seconds=20)): + raise RuntimeError("Unable to takeoff") + + is_ascending_to_takeoff_alt = True + while is_ascending_to_takeoff_alt: + rclpy.spin_once(node) + time.sleep(1.0) + + is_ascending_to_takeoff_alt = node.get_cur_geopose().pose.position.altitude < TAKEOFF_ALT + + if is_ascending_to_takeoff_alt: + raise RuntimeError("Failed to reach takeoff altitude") + + except KeyboardInterrupt: + pass + + # Destroy the node explicitly. + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/Tools/ros2/ardupilot_dds_tests/setup.py b/Tools/ros2/ardupilot_dds_tests/setup.py index bbe11e6a5c8123..a8f2e57fe25424 100644 --- a/Tools/ros2/ardupilot_dds_tests/setup.py +++ b/Tools/ros2/ardupilot_dds_tests/setup.py @@ -26,6 +26,7 @@ 'console_scripts': [ "time_listener = ardupilot_dds_tests.time_listener:main", "plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main", + "copter_takeoff = ardupilot_dds_tests.copter_takeoff:main", ], }, ) diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index d066a2d32b2e43..96fa09107575b3 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" + "srv/Takeoff.srv" DEPENDENCIES geometry_msgs std_msgs ADD_LINTER_TESTS ) diff --git a/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv new file mode 100644 index 00000000000000..879cc906fc7d73 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv @@ -0,0 +1,10 @@ + +# This service requests the vehicle to takeoff + +# alt : Set the takeoff altitude above home or above terrain(in case of rangefinder) + +uint8 alt +--- +# status : True if the request for mode switch was successful, False otherwise + +bool status diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 373d93bc62ecac..4d1abe4e8b79cc 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -17,6 +17,7 @@ #include "ardupilot_msgs/srv/ArmMotors.h" #include "ardupilot_msgs/srv/ModeSwitch.h" +#include "ardupilot_msgs/srv/Takeoff.h" #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" @@ -652,6 +653,33 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL"); break; } + case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: { + ardupilot_msgs_srv_Takeoff_Request takeoff_request; + ardupilot_msgs_srv_Takeoff_Response takeoff_response; + const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request); + if (deserialize_success == false) { + break; + } + takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt); + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id, + .type = UXR_REPLIER_ID + }; + + uint8_t reply_buffer[8] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); + const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL"); + break; + } } } diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 50611276a5539e..353067ab16a1b8 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -2,7 +2,8 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, - MODE_SWITCH + MODE_SWITCH, + TAKEOFF }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -24,4 +25,10 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .req_profile_label = "", .rep_profile_label = "mode_switch__replier", }, + { + .req_id = to_underlying(ServiceIndex::TAKEOFF), + .rep_id = to_underlying(ServiceIndex::TAKEOFF), + .req_profile_label = "", + .rep_profile_label = "takeoff__replier", + }, }; diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl new file mode 100644 index 00000000000000..0d56fd474a649e --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl @@ -0,0 +1,20 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from ardupilot_msgs/srv/Takeoff.srv +// generated code does not contain a copyright notice + + +module ardupilot_msgs { + module srv { + struct Takeoff_Request { + @verbatim (language="comment", text= + "This service requests the vehicle to takeoff" "\n" + "alt : Set the takeoff altitude") + uint8 alt; + }; + @verbatim (language="comment", text= + "status : True if the request for takeoff was successful, False otherwise") + struct Takeoff_Response { + boolean status; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index f08aaf12490d82..be91c64b727eff 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -240,6 +240,7 @@ nanosec: 729410000 $ ros2 service list /ap/arm_motors /ap/mode_switch +/ap/takeoff --- ``` @@ -265,6 +266,7 @@ List the available services: $ ros2 service list -t /ap/arm_motors [ardupilot_msgs/srv/ArmMotors] /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch] +/ap/takeoff [ardupilot_msgs/srv/Takeoff] ``` Call the arm motors service: @@ -286,6 +288,16 @@ requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4) response: ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4) ``` + +Call the takeoff service: + +```bash +$ ros2 service call /ap/takeoff ardupilot_msgs/srv/takeoff "{alt: 10}" +requester: making request: ardupilot_msgs.srv.Takeoff_Request(alt=10) + +response: +ardupilot_msgs.srv.Takeoff_Response(status=True) +``` ## Contributing to `AP_DDS` library diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 3dcece3f9d30cd..e7fb9d6b7291a6 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -327,6 +327,10 @@ rq/ap/mode_switchRequest rr/ap/mode_switchReply + + rq/ap/takeoffRequest + rr/ap/takeoffReply + rt/ap/cmd_gps_pose ardupilot_msgs::msg::dds_::GlobalPosition_