uorb: add message format compatibility check

This can be used by DDS/ROS 2 to check for matching message definitions.
This commit is contained in:
Beat Küng 2023-07-26 13:22:03 +02:00
parent 0dcecf0666
commit 22acb08406
9 changed files with 162 additions and 2 deletions

View File

@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
return spec_temp.parsed_fields()
def get_message_fields_str_for_message_hash(msg_fields, search_path):
"""
Get all fields (including for nested types) in the form of:
'''
uint64 timestamp
uint8 esc_count
uint8 esc_online_flags
EscReport[8] esc
uint64 timestamp
uint32 esc_errorcount
int32 esc_rpm
float32 esc_voltage
uint16 failures
int8 esc_power
'''
"""
all_fields_str = ''
for field in msg_fields:
if field.is_header:
continue
type_name = field.type
# detect embedded types
sl_pos = type_name.find('/')
if sl_pos >= 0:
type_name = type_name[sl_pos + 1:]
all_fields_str += type_name + ' ' + field.name + '\n'
if sl_pos >= 0: # nested type, add all nested fields
children_fields = get_children_fields(field.base_type, search_path)
all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path)
return all_fields_str
def hash_32_fnv1a(data: str):
hash_val = 0x811c9dc5
prime = 0x1000193
for i in range(len(data)):
value = ord(data[i])
hash_val = hash_val ^ value
hash_val *= prime
hash_val &= 0xffffffff
return hash_val
def get_message_hash(msg_fields, search_path):
"""
Get a 32 bit message hash over all fields
"""
all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path)
return hash_32_fnv1a(all_fields_str)
def add_padding_bytes(fields, search_path):
"""
Add padding fields before the embedded types, at the end and calculate the

View File

@ -58,6 +58,7 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
uorb_struct = '%s_s'%name_snake_case
message_hash = get_message_hash(spec.parsed_fields(), search_path)
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
}@
@ -74,7 +75,7 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
@[for topic in topics]@
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast<orb_id_size_t>(ORB_ID::@topic));
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
@[end for]
void print_message(const orb_metadata *meta, const @uorb_struct& message)

View File

@ -133,6 +133,8 @@ set(msg_files
ManualControlSwitches.msg
MavlinkLog.msg
MavlinkTunnel.msg
MessageFormatRequest.msg
MessageFormatResponse.msg
Mission.msg
MissionResult.msg
MountOrientation.msg

View File

@ -0,0 +1,10 @@
uint64 timestamp # time since system start (microseconds)
# Request to PX4 to get the hash of a message, to check for message compatibility
uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes.
uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
char[50] topic_name # E.g. /fmu/in/vehicle_command

View File

@ -0,0 +1,11 @@
uint64 timestamp # time since system start (microseconds)
# Response from PX4 with the format of a message
uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
char[50] topic_name # E.g. /fmu/in/vehicle_command
bool success
uint32 message_hash # hash over all message fields

View File

@ -51,6 +51,7 @@ struct orb_metadata {
const char *o_name; /**< unique object name */
const uint16_t o_size; /**< object size */
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
uint32_t message_hash; /**< Hash over all fields for message compatibility checks */
orb_id_size_t o_id; /**< ORB_ID enum */
};
@ -99,13 +100,15 @@ typedef const struct orb_metadata *orb_id_t;
* @param _name The name of the topic.
* @param _struct The structure the topic provides.
* @param _size_no_padding Struct size w/o padding at the end
* @param _message_hash 32 bit message hash over all fields
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
*/
#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum) \
#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct), \
_size_no_padding, \
_message_hash, \
_orb_id_enum \
}; struct hack

View File

@ -23,6 +23,9 @@ publications:
- topic: /fmu/out/manual_control_setpoint
type: px4_msgs::msg::ManualControlSetpoint
- topic: /fmu/out/message_format_response
type: px4_msgs::msg::MessageFormatResponse
- topic: /fmu/out/position_setpoint_triplet
type: px4_msgs::msg::PositionSetpointTriplet
@ -75,6 +78,9 @@ subscriptions:
- topic: /fmu/in/arming_check_reply
type: px4_msgs::msg::ArmingCheckReply
- topic: /fmu/in/message_format_request
type: px4_msgs::msg::MessageFormatRequest
- topic: /fmu/in/mode_completed
type: px4_msgs::msg::ModeCompleted

View File

@ -188,6 +188,67 @@ UxrceddsClient::~UxrceddsClient()
}
}
static void fillMessageFormatResponse(const message_format_request_s &message_format_request,
message_format_response_s &message_format_response)
{
message_format_response.protocol_version = message_format_request_s::LATEST_PROTOCOL_VERSION;
message_format_response.success = false;
if (message_format_request.protocol_version == message_format_request_s::LATEST_PROTOCOL_VERSION) {
static_assert(sizeof(message_format_request.topic_name) == sizeof(message_format_response.topic_name), "size mismatch");
memcpy(message_format_response.topic_name, message_format_request.topic_name,
sizeof(message_format_response.topic_name));
// Get the topic name by searching for the last '/'
int idx_last_slash = -1;
bool found_null = false;
for (int i = 0; i < (int)sizeof(message_format_request.topic_name); ++i) {
if (message_format_request.topic_name[i] == 0) {
found_null = true;
break;
}
if (message_format_request.topic_name[i] == '/') {
idx_last_slash = i;
}
}
if (found_null && idx_last_slash != -1) {
const char *topic_name = message_format_request.topic_name + idx_last_slash + 1;
// Find the format
const orb_metadata *const *topics = orb_get_topics();
const orb_metadata *topic_meta{nullptr};
for (size_t i = 0; i < orb_topics_count(); i++) {
if (strcmp(topic_name, topics[i]->o_name) == 0) {
topic_meta = topics[i];
break;
}
}
if (topic_meta) {
message_format_response.message_hash = topic_meta->message_hash;
// The topic type is already checked by DDS
message_format_response.success = true;
}
}
}
message_format_response.timestamp = hrt_absolute_time();
}
void UxrceddsClient::handleMessageFormatRequest()
{
message_format_request_s message_format_request;
if (_message_format_request_sub.update(&message_format_request)) {
message_format_response_s message_format_response;
fillMessageFormatResponse(message_format_request, message_format_response);
_message_format_response_pub.publish(message_format_response);
}
}
void UxrceddsClient::run()
{
if (!_comm) {
@ -405,6 +466,8 @@ void UxrceddsClient::run()
}
}
handleMessageFormatRequest();
// Check for a ping response
/* PONG_IN_SESSION_STATUS */
if (session.on_pong_flag == 1) {

View File

@ -38,6 +38,10 @@
#include <src/modules/uxrce_dds_client/dds_topics.h>
#include <uORB/topics/message_format_request.h>
#include <uORB/topics/message_format_response.h>
#include <uORB/Subscription.hpp>
#include <lib/timesync/Timesync.hpp>
#include "srv_base.h"
@ -106,6 +110,11 @@ public:
private:
int setBaudrate(int fd, unsigned baud);
void handleMessageFormatRequest();
uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
const bool _localhost_only;
const bool _custom_participant;
const char *_client_namespace;