Adding OpenDroneID plugin and messages

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

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()</description>
+  </class>
+  <class name="open_drone_id" type="mavros::plugin::PluginFactoryTemplate&lt;mavros::extra_plugins::OpenDroneIDPlugin&gt;" base_class_type="mavros::plugin::PluginFactory">
+    <description>@brief Open Drone ID plugin
+@plugin open_drone_id
+
+Sends Open Drone ID data to the FCU</description>
   </class>
   <class name="optical_flow" type="mavros::plugin::PluginFactoryTemplate&lt;mavros::extra_plugins::OpticalFlowPlugin&gt;" base_class_type="mavros::plugin::PluginFactory">
     <description>@brief Optical Flow custom plugin

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 <[email protected]>
+ *
+ * @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<mavros_msgs::msg::BasicID>(
+			"~/basic_id", 1, std::bind(
+			&OpenDroneIDPlugin::basic_id_cb, this,
+			_1));
+
+		operator_id_sub = node->create_subscription<mavros_msgs::msg::OperatorID>(
+			"~/operator_id", 1, std::bind(
+			&OpenDroneIDPlugin::operator_id_cb, this,
+			_1));
+
+		self_id_sub = node->create_subscription<mavros_msgs::msg::SelfID>(
+			"~/self_id", 1, std::bind(
+			&OpenDroneIDPlugin::self_id_cb, this,
+			_1));
+
+		system_sub = node->create_subscription<mavros_msgs::msg::System>(
+			"~/system", 1, std::bind(
+			&OpenDroneIDPlugin::system_cb, this,
+			_1));
+
+		system_update_sub = node->create_subscription<mavros_msgs::msg::SystemUpdate>(
+			"~/system_update", 1, std::bind(
+			&OpenDroneIDPlugin::system_update_cb, this,
+			_1));
+	}
+
+	Subscriptions get_subscriptions()
+	{
+		return { };
+	}
+
+private:
+	rclcpp::Subscription<mavros_msgs::msg::BasicID>::SharedPtr basic_id_sub;
+	rclcpp::Subscription<mavros_msgs::msg::OperatorID>::SharedPtr operator_id_sub;
+	rclcpp::Subscription<mavros_msgs::msg::SelfID>::SharedPtr self_id_sub;
+	rclcpp::Subscription<mavros_msgs::msg::System>::SharedPtr system_sub;
+	rclcpp::Subscription<mavros_msgs::msg::SystemUpdate>::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 <mavros/mavros_plugin_register_macro.hpp>  // NOLINT
+MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::OpenDroneIDPlugin)

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

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

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

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

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

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