Skip to content

Commit 2ae9759

Browse files
gus-robotics88vooon
gus-robotics88
authored and
vooon
committed
Adding OpenDroneID plugin and messages (#3)
1 parent 7a64c81 commit 2ae9759

File tree

9 files changed

+247
-0
lines changed

9 files changed

+247
-0
lines changed

‎mavros_extras/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,7 @@ add_library(mavros_extras_plugins SHARED
117117
src/plugins/obstacle_distance.cpp
118118
src/plugins/odom.cpp
119119
src/plugins/onboard_computer_status.cpp
120+
src/plugins/open_drone_id.cpp
120121
src/plugins/optical_flow.cpp
121122
src/plugins/play_tune.cpp
122123
src/plugins/px4flow.cpp

‎mavros_extras/mavros_plugins.xml

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -164,6 +164,12 @@ The twist is expressed in the child frame.
164164

165165
Publishes the status of the onboard computer
166166
@see status_cb()</description>
167+
</class>
168+
<class name="open_drone_id" type="mavros::plugin::PluginFactoryTemplate&lt;mavros::extra_plugins::OpenDroneIDPlugin&gt;" base_class_type="mavros::plugin::PluginFactory">
169+
<description>@brief Open Drone ID plugin
170+
@plugin open_drone_id
171+
172+
Sends Open Drone ID data to the FCU</description>
167173
</class>
168174
<class name="optical_flow" type="mavros::plugin::PluginFactoryTemplate&lt;mavros::extra_plugins::OpticalFlowPlugin&gt;" base_class_type="mavros::plugin::PluginFactory">
169175
<description>@brief Optical Flow custom plugin

‎mavros_extras/src/plugins/open_drone_id.cpp

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,156 @@
1+
/*
2+
* Copyright 2024 Gus Meyer.
3+
*
4+
* This file is part of the mavros package and subject to the license terms
5+
* in the top-level LICENSE file of the mavros repository.
6+
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
7+
*/
8+
/**
9+
* @brief Open Drone ID plugin to satisfy remote ID requirements
10+
* @file open_drone_id.cpp
11+
* @author Gus Meyer <[email protected]>
12+
*
13+
* @addtogroup plugin
14+
* @{
15+
*/
16+
17+
18+
#include "rcpputils/asserts.hpp"
19+
#include "mavros/mavros_uas.hpp"
20+
#include "mavros/plugin.hpp"
21+
#include "mavros/plugin_filter.hpp"
22+
23+
#include "mavros_msgs/msg/basic_id.hpp"
24+
#include "mavros_msgs/msg/operator_id.hpp"
25+
#include "mavros_msgs/msg/self_id.hpp"
26+
#include "mavros_msgs/msg/system.hpp"
27+
#include "mavros_msgs/msg/system_update.hpp"
28+
29+
namespace mavros {
30+
namespace extra_plugins {
31+
using namespace std::placeholders; // NOLINT
32+
33+
/**
34+
* @brief Open Drone ID plugin
35+
*
36+
* Sends Open Drone ID data to the FCU
37+
*/
38+
class OpenDroneIDPlugin : public plugin::Plugin
39+
{
40+
public:
41+
explicit OpenDroneIDPlugin(plugin::UASPtr uas_)
42+
: Plugin(uas_, "open_drone_id")
43+
{
44+
basic_id_sub = node->create_subscription<mavros_msgs::msg::BasicID>(
45+
"~/basic_id", 1, std::bind(
46+
&OpenDroneIDPlugin::basic_id_cb, this,
47+
_1));
48+
49+
operator_id_sub = node->create_subscription<mavros_msgs::msg::OperatorID>(
50+
"~/operator_id", 1, std::bind(
51+
&OpenDroneIDPlugin::operator_id_cb, this,
52+
_1));
53+
54+
self_id_sub = node->create_subscription<mavros_msgs::msg::SelfID>(
55+
"~/self_id", 1, std::bind(
56+
&OpenDroneIDPlugin::self_id_cb, this,
57+
_1));
58+
59+
system_sub = node->create_subscription<mavros_msgs::msg::System>(
60+
"~/system", 1, std::bind(
61+
&OpenDroneIDPlugin::system_cb, this,
62+
_1));
63+
64+
system_update_sub = node->create_subscription<mavros_msgs::msg::SystemUpdate>(
65+
"~/system_update", 1, std::bind(
66+
&OpenDroneIDPlugin::system_update_cb, this,
67+
_1));
68+
}
69+
70+
Subscriptions get_subscriptions()
71+
{
72+
return { };
73+
}
74+
75+
private:
76+
rclcpp::Subscription<mavros_msgs::msg::BasicID>::SharedPtr basic_id_sub;
77+
rclcpp::Subscription<mavros_msgs::msg::OperatorID>::SharedPtr operator_id_sub;
78+
rclcpp::Subscription<mavros_msgs::msg::SelfID>::SharedPtr self_id_sub;
79+
rclcpp::Subscription<mavros_msgs::msg::System>::SharedPtr system_sub;
80+
rclcpp::Subscription<mavros_msgs::msg::SystemUpdate>::SharedPtr system_update_sub;
81+
82+
83+
void basic_id_cb(const mavros_msgs::msg::BasicID::SharedPtr msg)
84+
{
85+
mavlink::common::msg::OPEN_DRONE_ID_BASIC_ID basic_id{};
86+
87+
basic_id.id_type = msg->id_type;
88+
basic_id.ua_type = msg->ua_type;
89+
90+
size_t length = std::min(basic_id.uas_id.size(), msg->uas_id.size());
91+
std::memcpy(basic_id.uas_id.data(), msg->uas_id.data(), length);
92+
93+
uas->send_message(basic_id);
94+
}
95+
96+
void operator_id_cb(const mavros_msgs::msg::OperatorID::SharedPtr msg)
97+
{
98+
mavlink::common::msg::OPEN_DRONE_ID_OPERATOR_ID operator_id{};
99+
100+
operator_id.operator_id_type = msg->operator_id_type;
101+
102+
size_t length = std::min(operator_id.operator_id.size(), msg->operator_id.size());
103+
std::memcpy(operator_id.operator_id.data(), msg->operator_id.data(), length);
104+
105+
uas->send_message(operator_id);
106+
}
107+
108+
void self_id_cb(const mavros_msgs::msg::SelfID::SharedPtr msg)
109+
{
110+
mavlink::common::msg::OPEN_DRONE_ID_SELF_ID self_id{};
111+
self_id.description_type = msg->description_type;
112+
113+
size_t length = std::min(self_id.description.size(), msg->description.size());
114+
std::memcpy(self_id.description.data(), msg->description.data(), length);
115+
116+
uas->send_message(self_id);
117+
}
118+
119+
void system_cb(const mavros_msgs::msg::System::SharedPtr msg)
120+
{
121+
mavlink::common::msg::OPEN_DRONE_ID_SYSTEM system{};
122+
123+
system.operator_location_type = msg->operator_location_type;
124+
system.classification_type = msg->classification_type;
125+
system.operator_latitude = msg->operator_latitude;
126+
system.operator_longitude = msg->operator_longitude;
127+
system.area_count = msg->area_count;
128+
system.area_radius = msg->area_radius;
129+
system.area_ceiling = msg->area_ceiling;
130+
system.area_floor = msg->area_floor;
131+
system.category_eu = msg->category_eu;
132+
system.class_eu = msg->class_eu;
133+
system.operator_altitude_geo = msg->operator_altitude_geo;
134+
system.timestamp = msg->timestamp;
135+
136+
uas->send_message(system);
137+
}
138+
139+
void system_update_cb(const mavros_msgs::msg::SystemUpdate::SharedPtr msg)
140+
{
141+
mavlink::common::msg::OPEN_DRONE_ID_SYSTEM_UPDATE system_update{};
142+
143+
system_update.operator_latitude = msg->operator_latitude;
144+
system_update.operator_longitude = msg->operator_longitude;
145+
system_update.operator_altitude_geo = msg->operator_altitude_geo;
146+
system_update.timestamp = msg->timestamp;
147+
148+
uas->send_message(system_update);
149+
}
150+
151+
};
152+
} // namespace extra_plugins
153+
} // namespace mavros
154+
155+
#include <mavros/mavros_plugin_register_macro.hpp> // NOLINT
156+
MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::OpenDroneIDPlugin)

‎mavros_msgs/CMakeLists.txt

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,11 @@ set(msg_files
7676
msg/MountControl.msg
7777
msg/NavControllerOutput.msg
7878
msg/OnboardComputerStatus.msg
79+
msg/OpenDroneID/BasicID.msg
80+
msg/OpenDroneID/OperatorID.msg
81+
msg/OpenDroneID/SelfID.msg
82+
msg/OpenDroneID/System.msg
83+
msg/OpenDroneID/SystemUpdate.msg
7984
msg/OpticalFlow.msg
8085
msg/OpticalFlowRad.msg
8186
msg/OverrideRCIn.msg

‎mavros_msgs/msg/OpenDroneID/BasicID.msg

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
# Remote ID message - Basic ID
2+
3+
std_msgs/Header header
4+
5+
uint8 id_type
6+
uint8 MAV_ODID_ID_TYPE_NONE = 0
7+
uint8 MAV_ODID_ID_TYPE_SERIAL_NUMBER = 1
8+
uint8 MAV_ODID_ID_TYPE_CAA_REGISTRATION_ID = 2
9+
uint8 MAV_ODID_ID_TYPE_UTM_ASSIGNED_UUID = 3
10+
uint8 MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID = 4
11+
12+
uint8 ua_type
13+
uint8 MAV_ODID_UA_TYPE_NONE = 0
14+
uint8 MAV_ODID_UA_TYPE_AEROPLANE = 1
15+
uint8 MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR = 2
16+
uint8 MAV_ODID_UA_TYPE_GYROPLANE = 3
17+
uint8 MAV_ODID_UA_TYPE_HYBRID_LIFT = 4
18+
uint8 MAV_ODID_UA_TYPE_ORNITHOPTER = 5
19+
uint8 MAV_ODID_UA_TYPE_GLIDER = 6
20+
uint8 MAV_ODID_UA_TYPE_KITE = 7
21+
uint8 MAV_ODID_UA_TYPE_FREE_BALLOON = 8
22+
uint8 MAV_ODID_UA_TYPE_CAPTIVE_BALLOON = 9
23+
uint8 MAV_ODID_UA_TYPE_AIRSHIP = 10
24+
uint8 MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE = 11
25+
uint8 MAV_ODID_UA_TYPE_ROCKET = 12
26+
uint8 MAV_ODID_UA_TYPE_TETHERED_POWERED_AIRCRAFT = 13
27+
uint8 MAV_ODID_UA_TYPE_GROUND_OBSTACLE = 14
28+
uint8 MAV_ODID_UA_TYPE_OTHER = 15
29+
30+
string uas_id

‎mavros_msgs/msg/OpenDroneID/OperatorID.msg

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# Remote ID message - Operator ID
2+
3+
std_msgs/Header header
4+
5+
uint8 operator_id_type
6+
uint8 MAV_ODID_OPERATOR_ID_TYPE_CAA = 0
7+
8+
string operator_id

‎mavros_msgs/msg/OpenDroneID/SelfID.msg

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
# Remote ID message - Self ID
2+
3+
std_msgs/Header header
4+
5+
uint8 description_type
6+
uint8 MAV_ODID_DESC_TYPE_TEXT = 0
7+
uint8 MAV_ODID_DESC_TYPE_EMERGENCY = 1
8+
uint8 MAV_ODID_DESC_TYPE_EXTENDED_STATUS = 2
9+
10+
string description

‎mavros_msgs/msg/OpenDroneID/System.msg

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Remote ID message - System
2+
3+
std_msgs/Header header
4+
5+
uint8 operator_location_type
6+
uint8 MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF = 0
7+
uint8 MAV_ODID_OPERATOR_LOCATION_TYPE_LIVE_GNSS = 1
8+
uint8 MAV_ODID_OPERATOR_LOCATION_TYPE_FIXED = 2
9+
10+
uint8 classification_type
11+
uint8 MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED = 0
12+
uint8 MAV_ODID_CLASSIFICATION_TYPE_EU = 1
13+
14+
int32 operator_latitude
15+
int32 operator_longitude
16+
uint16 area_count
17+
uint16 area_radius
18+
float32 area_ceiling
19+
float32 area_floor
20+
uint8 category_eu
21+
uint8 class_eu
22+
float32 operator_altitude_geo
23+
uint32 timestamp

‎mavros_msgs/msg/OpenDroneID/SystemUpdate.msg

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# Remote ID message - System Update
2+
3+
std_msgs/Header header
4+
5+
int32 operator_latitude
6+
int32 operator_longitude
7+
float32 operator_altitude_geo
8+
uint32 timestamp

0 commit comments

Comments
 (0)