diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index 0c6675a7f..765144c2c 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -111,6 +111,7 @@ add_library(mavros_extras_plugins SHARED src/plugins/obstacle_distance.cpp src/plugins/odom.cpp src/plugins/onboard_computer_status.cpp + src/plugins/open_drone_id.cpp src/plugins/optical_flow.cpp src/plugins/play_tune.cpp src/plugins/px4flow.cpp diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index a0a7032b8..bfd1206a4 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -157,6 +157,12 @@ The twist is expressed in the child frame. Publishes the status of the onboard computer @see status_cb() + + + @brief Open Drone ID plugin +@plugin open_drone_id + +Sends Open Drone ID data to the FCU @brief Optical Flow custom plugin diff --git a/mavros_extras/src/plugins/open_drone_id.cpp b/mavros_extras/src/plugins/open_drone_id.cpp new file mode 100644 index 000000000..bad26d1c1 --- /dev/null +++ b/mavros_extras/src/plugins/open_drone_id.cpp @@ -0,0 +1,156 @@ +/* + * Copyright 2024 Gus Meyer. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Open Drone ID plugin to satisfy remote ID requirements + * @file open_drone_id.cpp + * @author Gus Meyer + * + * @addtogroup plugin + * @{ + */ + + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/msg/basic_id.hpp" +#include "mavros_msgs/msg/operator_id.hpp" +#include "mavros_msgs/msg/self_id.hpp" +#include "mavros_msgs/msg/system.hpp" +#include "mavros_msgs/msg/system_update.hpp" + +namespace mavros { +namespace extra_plugins { +using namespace std::placeholders; // NOLINT + +/** + * @brief Open Drone ID plugin + * + * Sends Open Drone ID data to the FCU + */ +class OpenDroneIDPlugin : public plugin::Plugin +{ +public: + explicit OpenDroneIDPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "open_drone_id") + { + basic_id_sub = node->create_subscription( + "~/basic_id", 1, std::bind( + &OpenDroneIDPlugin::basic_id_cb, this, + _1)); + + operator_id_sub = node->create_subscription( + "~/operator_id", 1, std::bind( + &OpenDroneIDPlugin::operator_id_cb, this, + _1)); + + self_id_sub = node->create_subscription( + "~/self_id", 1, std::bind( + &OpenDroneIDPlugin::self_id_cb, this, + _1)); + + system_sub = node->create_subscription( + "~/system", 1, std::bind( + &OpenDroneIDPlugin::system_cb, this, + _1)); + + system_update_sub = node->create_subscription( + "~/system_update", 1, std::bind( + &OpenDroneIDPlugin::system_update_cb, this, + _1)); + } + + Subscriptions get_subscriptions() + { + return { }; + } + +private: + rclcpp::Subscription::SharedPtr basic_id_sub; + rclcpp::Subscription::SharedPtr operator_id_sub; + rclcpp::Subscription::SharedPtr self_id_sub; + rclcpp::Subscription::SharedPtr system_sub; + rclcpp::Subscription::SharedPtr system_update_sub; + + + void basic_id_cb(const mavros_msgs::msg::BasicID::SharedPtr msg) + { + mavlink::common::msg::OPEN_DRONE_ID_BASIC_ID basic_id{}; + + basic_id.id_type = msg->id_type; + basic_id.ua_type = msg->ua_type; + + size_t length = std::min(basic_id.uas_id.size(), msg->uas_id.size()); + std::memcpy(basic_id.uas_id.data(), msg->uas_id.data(), length); + + uas->send_message(basic_id); + } + + void operator_id_cb(const mavros_msgs::msg::OperatorID::SharedPtr msg) + { + mavlink::common::msg::OPEN_DRONE_ID_OPERATOR_ID operator_id{}; + + operator_id.operator_id_type = msg->operator_id_type; + + size_t length = std::min(operator_id.operator_id.size(), msg->operator_id.size()); + std::memcpy(operator_id.operator_id.data(), msg->operator_id.data(), length); + + uas->send_message(operator_id); + } + + void self_id_cb(const mavros_msgs::msg::SelfID::SharedPtr msg) + { + mavlink::common::msg::OPEN_DRONE_ID_SELF_ID self_id{}; + self_id.description_type = msg->description_type; + + size_t length = std::min(self_id.description.size(), msg->description.size()); + std::memcpy(self_id.description.data(), msg->description.data(), length); + + uas->send_message(self_id); + } + + void system_cb(const mavros_msgs::msg::System::SharedPtr msg) + { + mavlink::common::msg::OPEN_DRONE_ID_SYSTEM system{}; + + system.operator_location_type = msg->operator_location_type; + system.classification_type = msg->classification_type; + system.operator_latitude = msg->operator_latitude; + system.operator_longitude = msg->operator_longitude; + system.area_count = msg->area_count; + system.area_radius = msg->area_radius; + system.area_ceiling = msg->area_ceiling; + system.area_floor = msg->area_floor; + system.category_eu = msg->category_eu; + system.class_eu = msg->class_eu; + system.operator_altitude_geo = msg->operator_altitude_geo; + system.timestamp = msg->timestamp; + + uas->send_message(system); + } + + void system_update_cb(const mavros_msgs::msg::SystemUpdate::SharedPtr msg) + { + mavlink::common::msg::OPEN_DRONE_ID_SYSTEM_UPDATE system_update{}; + + system_update.operator_latitude = msg->operator_latitude; + system_update.operator_longitude = msg->operator_longitude; + system_update.operator_altitude_geo = msg->operator_altitude_geo; + system_update.timestamp = msg->timestamp; + + uas->send_message(system_update); + } + +}; +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::OpenDroneIDPlugin) diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index 9be0db129..b637a3c7f 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -69,6 +69,11 @@ set(msg_files msg/MountControl.msg msg/NavControllerOutput.msg msg/OnboardComputerStatus.msg + msg/OpenDroneID/BasicID.msg + msg/OpenDroneID/OperatorID.msg + msg/OpenDroneID/SelfID.msg + msg/OpenDroneID/System.msg + msg/OpenDroneID/SystemUpdate.msg msg/OpticalFlow.msg msg/OpticalFlowRad.msg msg/OverrideRCIn.msg diff --git a/mavros_msgs/msg/OpenDroneID/BasicID.msg b/mavros_msgs/msg/OpenDroneID/BasicID.msg new file mode 100644 index 000000000..0e1c46d30 --- /dev/null +++ b/mavros_msgs/msg/OpenDroneID/BasicID.msg @@ -0,0 +1,30 @@ +# Remote ID message - Basic ID + +std_msgs/Header header + +uint8 id_type +uint8 MAV_ODID_ID_TYPE_NONE = 0 +uint8 MAV_ODID_ID_TYPE_SERIAL_NUMBER = 1 +uint8 MAV_ODID_ID_TYPE_CAA_REGISTRATION_ID = 2 +uint8 MAV_ODID_ID_TYPE_UTM_ASSIGNED_UUID = 3 +uint8 MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID = 4 + +uint8 ua_type +uint8 MAV_ODID_UA_TYPE_NONE = 0 +uint8 MAV_ODID_UA_TYPE_AEROPLANE = 1 +uint8 MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR = 2 +uint8 MAV_ODID_UA_TYPE_GYROPLANE = 3 +uint8 MAV_ODID_UA_TYPE_HYBRID_LIFT = 4 +uint8 MAV_ODID_UA_TYPE_ORNITHOPTER = 5 +uint8 MAV_ODID_UA_TYPE_GLIDER = 6 +uint8 MAV_ODID_UA_TYPE_KITE = 7 +uint8 MAV_ODID_UA_TYPE_FREE_BALLOON = 8 +uint8 MAV_ODID_UA_TYPE_CAPTIVE_BALLOON = 9 +uint8 MAV_ODID_UA_TYPE_AIRSHIP = 10 +uint8 MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE = 11 +uint8 MAV_ODID_UA_TYPE_ROCKET = 12 +uint8 MAV_ODID_UA_TYPE_TETHERED_POWERED_AIRCRAFT = 13 +uint8 MAV_ODID_UA_TYPE_GROUND_OBSTACLE = 14 +uint8 MAV_ODID_UA_TYPE_OTHER = 15 + +string uas_id diff --git a/mavros_msgs/msg/OpenDroneID/OperatorID.msg b/mavros_msgs/msg/OpenDroneID/OperatorID.msg new file mode 100644 index 000000000..68842993c --- /dev/null +++ b/mavros_msgs/msg/OpenDroneID/OperatorID.msg @@ -0,0 +1,8 @@ +# Remote ID message - Operator ID + +std_msgs/Header header + +uint8 operator_id_type +uint8 MAV_ODID_OPERATOR_ID_TYPE_CAA = 0 + +string operator_id diff --git a/mavros_msgs/msg/OpenDroneID/SelfID.msg b/mavros_msgs/msg/OpenDroneID/SelfID.msg new file mode 100644 index 000000000..b27c4629a --- /dev/null +++ b/mavros_msgs/msg/OpenDroneID/SelfID.msg @@ -0,0 +1,10 @@ +# Remote ID message - Self ID + +std_msgs/Header header + +uint8 description_type +uint8 MAV_ODID_DESC_TYPE_TEXT = 0 +uint8 MAV_ODID_DESC_TYPE_EMERGENCY = 1 +uint8 MAV_ODID_DESC_TYPE_EXTENDED_STATUS = 2 + +string description diff --git a/mavros_msgs/msg/OpenDroneID/System.msg b/mavros_msgs/msg/OpenDroneID/System.msg new file mode 100644 index 000000000..9c46af2f1 --- /dev/null +++ b/mavros_msgs/msg/OpenDroneID/System.msg @@ -0,0 +1,23 @@ +# Remote ID message - System + +std_msgs/Header header + +uint8 operator_location_type +uint8 MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF = 0 +uint8 MAV_ODID_OPERATOR_LOCATION_TYPE_LIVE_GNSS = 1 +uint8 MAV_ODID_OPERATOR_LOCATION_TYPE_FIXED = 2 + +uint8 classification_type +uint8 MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED = 0 +uint8 MAV_ODID_CLASSIFICATION_TYPE_EU = 1 + +int32 operator_latitude +int32 operator_longitude +uint16 area_count +uint16 area_radius +float32 area_ceiling +float32 area_floor +uint8 category_eu +uint8 class_eu +float32 operator_altitude_geo +uint32 timestamp diff --git a/mavros_msgs/msg/OpenDroneID/SystemUpdate.msg b/mavros_msgs/msg/OpenDroneID/SystemUpdate.msg new file mode 100644 index 000000000..4019dfcfb --- /dev/null +++ b/mavros_msgs/msg/OpenDroneID/SystemUpdate.msg @@ -0,0 +1,8 @@ +# Remote ID message - System Update + +std_msgs/Header header + +int32 operator_latitude +int32 operator_longitude +float32 operator_altitude_geo +uint32 timestamp