diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index add71875dcfc6e..c401115c79aaa9 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -275,36 +275,36 @@ constexpr int8_t Copter::_failsafe_priorities[7]; #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED -// set target location (for use by external control and scripting) -bool Copter::set_target_location(const Location& target_loc) +// start takeoff to given altitude (for use by scripting) +bool Copter::start_takeoff(float alt) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } - return mode_guided.set_destination(target_loc); + if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { + copter.set_auto_armed(true); + return true; + } + return false; } -#endif //MODE_GUIDED_ENABLED -#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED -#if AP_SCRIPTING_ENABLED -#if MODE_GUIDED_ENABLED -// start takeoff to given altitude (for use by scripting) -bool Copter::start_takeoff(float alt) +// set target location (for use by external control and scripting) +bool Copter::set_target_location(const Location& target_loc) { // exit if vehicle is not in Guided mode or Auto-Guided mode if (!flightmode->in_guided_mode()) { return false; } - if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { - copter.set_auto_armed(true); - return true; - } - return false; + return mode_guided.set_destination(target_loc); } +#endif //MODE_GUIDED_ENABLED +#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +#if AP_SCRIPTING_ENABLED +#if MODE_GUIDED_ENABLED // set target position (for use by scripting) bool Copter::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) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8a9822d1651790..b1aeb6c56a6a09 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -666,13 +666,13 @@ class Copter : public AP_Vehicle { uint32_t &log_bit) override; #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED + bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; #endif // MODE_GUIDED_ENABLED #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if AP_SCRIPTING_ENABLED #if MODE_GUIDED_ENABLED - bool start_takeoff(float alt) override; bool get_target_location(Location& target_loc) override; bool update_target_location(const Location &old_loc, const Location &new_loc) override; 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) override; 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 77f052de6204d6..e019a8c9844f9d 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" @@ -679,6 +680,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 22f6716beffd0a..4478339bdca6f7 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) @@ -38,4 +39,14 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .request_topic_name = "rq/ap/mode_switchRequest", .reply_topic_name = "rr/ap/mode_switchReply", }, + { + .req_id = to_underlying(ServiceIndex::TAKEOFF), + .rep_id = to_underlying(ServiceIndex::TAKEOFF), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/takeoffService", + .request_type = "ardupilot_msgs::srv::dds_::Takeoff_Request_", + .reply_type = "ardupilot_msgs::srv::dds_::Takeoff_Response_", + .request_topic_name = "rq/ap/takeoffRequest", + .reply_topic_name = "rr/ap/takeoffReply", + }, }; 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 6d0c39e154aacb..ceeb0c2f9e82bf 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -207,6 +207,7 @@ nanosec: 729410000 $ ros2 service list /ap/arm_motors /ap/mode_switch +/ap/takeoff --- ``` @@ -232,6 +233,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: @@ -253,6 +255,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_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 5256dd82e281f9..2d670419adb2b9 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -172,6 +172,8 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool is_crashed() const; #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED + // Method to takeoff for use by external control + virtual bool start_takeoff(float alt) { return false; } // Method to control vehicle position for use by external control virtual bool set_target_location(const Location& target_loc) { return false; } #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED @@ -179,7 +181,6 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { /* methods to control vehicle for use by scripting */ - virtual bool start_takeoff(float alt) { 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; }