Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DDS: Improve subscriber safety 

* Remove unused void* functions
* Switch subscriber handling based on ID
* Use enums instead of relying on manual alignment of indices in code
* Pass topic by reference using local stack
* Relates to #23964

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-06-03 22:23:53 -06:00 committed by Peter Barker
parent 7ab625b969
commit da2beb1ec1
4 changed files with 82 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -410,21 +410,27 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui
1) Store the read contents into the ucdr buffer
2) Deserialize the said contents into the topic instance
*/
sensor_msgs_msg_Joy* topic = nullptr;
const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, topic);
if (success == false || topic == nullptr) {
return;
switch (object_id.id) {
case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id:
sensor_msgs_msg_Joy topic;
const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &topic);
if (success == false) {
break;
}
uint32_t* count_ptr = (uint32_t*) args;
(*count_ptr)++;
if (topic.axes_size >= 4) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f",
topic.axes[0], topic.axes[1], topic.axes[2], topic.axes[3]);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: Insufficient axes size ");
}
break;
}
uint32_t* count_ptr = (uint32_t*) args;
(*count_ptr)++;
if (topic->axes_size >= 4) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Receivied sensor_msgs/Joy: %f, %f, %f, %f",
topic->axes[0], topic->axes[1], topic->axes[2], topic->axes[3]);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Receivied sensor_msgs/Joy: Insufficient axes size ");
}
}
/*

4
libraries/AP_DDS/AP_DDS_Client.h
View File

@ -5,7 +5,6 @@
#include "uxr/client/client.h"
#include "ucdr/microcdr.h"
#include "builtin_interfaces/msg/Time.h"
#include "AP_DDS_Generic_Fn_T.h"
#include "sensor_msgs/msg/NavSatFix.h"
#include "tf2_msgs/msg/TFMessage.h"
@ -185,9 +184,6 @@ public:
const char* topic_profile_label;
const char* dw_profile_label;
const char* dr_profile_label;
Generic_serialize_topic_fn_t serialize;
Generic_deserialize_topic_fn_t deserialize;
Generic_size_of_topic_fn_t size_of;
};
static const struct Topic_table topics[];

7
libraries/AP_DDS/AP_DDS_Generic_Fn_T.h
View File

@ -1,7 +0,0 @@
#pragma once
// table maps between string names and pointer to element
// Function pointer that matches signature of generated topics
typedef bool (*Generic_serialize_topic_fn_t)(struct ucdrBuffer*, const void*);
typedef bool (*Generic_deserialize_topic_fn_t)(struct ucdrBuffer*, void*);
typedef uint32_t (*Generic_size_of_topic_fn_t)(struct ucdrBuffer*, uint32_t);

138
libraries/AP_DDS/AP_DDS_Topic_Table.h
View File

@ -4,130 +4,120 @@
#include "sensor_msgs/msg/BatteryState.h"
#include "geographic_msgs/msg/GeoPoseStamped.h"
#include "AP_DDS_Generic_Fn_T.h"
#include "uxr/client/client.h"
// Code generated table based on the enabled topics.
// Mavgen is using python, loops are not readable.
// Can use jinja to template (like Flask)
enum class TopicIndex: uint8_t {
TIME_PUB,
NAV_SAT_FIX_PUB,
STATIC_TRANSFORMS_PUB,
BATTERY_STATE_PUB,
LOCAL_POSE_PUB,
LOCAL_VELOCITY_PUB,
GEOPOSE_PUB,
CLOCK_PUB,
JOY_SUB,
};
const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
static inline constexpr uint8_t to_underlying(const TopicIndex index)
{
static_assert(sizeof(index) == sizeof(uint8_t));
return static_cast<uint8_t>(index);
}
constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.topic_id = 0x01,
.pub_id = 0x01,
.sub_id = 0x01,
.dw_id = uxrObjectId{.id=0x01, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x01, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::TIME_PUB),
.pub_id = to_underlying(TopicIndex::TIME_PUB),
.sub_id = to_underlying(TopicIndex::TIME_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "time__t",
.dw_profile_label = "time__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&builtin_interfaces_msg_Time_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&builtin_interfaces_msg_Time_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&builtin_interfaces_msg_Time_size_of_topic),
},
{
.topic_id = 0x02,
.pub_id = 0x02,
.sub_id = 0x02,
.dw_id = uxrObjectId{.id=0x02, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x02, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB),
.pub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB),
.sub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "navsatfix0__t",
.dw_profile_label = "navsatfix0__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_NavSatFix_size_of_topic),
},
{
.topic_id = 0x03,
.pub_id = 0x03,
.sub_id = 0x03,
.dw_id = uxrObjectId{.id=0x03, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x03, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB),
.pub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB),
.sub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "statictransforms__t",
.dw_profile_label = "statictransforms__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&tf2_msgs_msg_TFMessage_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&tf2_msgs_msg_TFMessage_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&tf2_msgs_msg_TFMessage_size_of_topic),
},
{
.topic_id = 0x04,
.pub_id = 0x04,
.sub_id = 0x04,
.dw_id = uxrObjectId{.id=0x04, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x04, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::BATTERY_STATE_PUB),
.pub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB),
.sub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "batterystate0__t",
.dw_profile_label = "batterystate0__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_BatteryState_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_BatteryState_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_BatteryState_size_of_topic),
},
{
.topic_id = 0x05,
.pub_id = 0x05,
.sub_id = 0x05,
.dw_id = uxrObjectId{.id=0x05, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x05, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
.pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
.sub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "localpose__t",
.dw_profile_label = "localpose__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_PoseStamped_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_PoseStamped_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_PoseStamped_size_of_topic),
},
{
.topic_id = 0x06,
.pub_id = 0x06,
.sub_id = 0x06,
.dw_id = uxrObjectId{.id=0x06, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x06, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB),
.pub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB),
.sub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "localvelocity__t",
.dw_profile_label = "localvelocity__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_TwistStamped_size_of_topic),
},
{
.topic_id = 0x07,
.pub_id = 0x07,
.sub_id = 0x07,
.dw_id = uxrObjectId{.id=0x07, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x07, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::GEOPOSE_PUB),
.pub_id = to_underlying(TopicIndex::GEOPOSE_PUB),
.sub_id = to_underlying(TopicIndex::GEOPOSE_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "geopose__t",
.dw_profile_label = "geopose__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_size_of_topic),
},
{
.topic_id = 0x08,
.pub_id = 0x08,
.sub_id = 0x08,
.dw_id = uxrObjectId{.id=0x08, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x08, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::CLOCK_PUB),
.pub_id = to_underlying(TopicIndex::CLOCK_PUB),
.sub_id = to_underlying(TopicIndex::CLOCK_PUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "clock__t",
.dw_profile_label = "clock__dw",
.dr_profile_label = "",
.serialize = Generic_serialize_topic_fn_t(&rosgraph_msgs_msg_Clock_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&rosgraph_msgs_msg_Clock_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&rosgraph_msgs_msg_Clock_size_of_topic),
},
{
.topic_id = 0x09,
.pub_id = 0x09,
.sub_id = 0x09,
.dw_id = uxrObjectId{.id=0x09, .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=0x09, .type=UXR_DATAREADER_ID},
.topic_id = to_underlying(TopicIndex::JOY_SUB),
.pub_id = to_underlying(TopicIndex::JOY_SUB),
.sub_id = to_underlying(TopicIndex::JOY_SUB),
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAWRITER_ID},
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAREADER_ID},
.topic_profile_label = "joy__t",
.dw_profile_label = "",
.dr_profile_label = "joy__dr",
.serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_Joy_serialize_topic),
.deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_Joy_deserialize_topic),
.size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_Joy_size_of_topic),
},
};