mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 15:53:34 +08:00
microdds: add xrce client
This commit is contained in:
Daniel Mesham
@@ -260,14 +260,17 @@ endif()
|
||||
|
||||
# headers
|
||||
set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics)
|
||||
set(ucdr_out_path ${PX4_BINARY_DIR}/uORB/ucdr)
|
||||
set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources)
|
||||
|
||||
set(uorb_headers ${msg_out_path}/uORBTopics.hpp)
|
||||
set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp)
|
||||
set(uorb_ucdr_headers)
|
||||
foreach(msg_file ${msg_files})
|
||||
get_filename_component(msg ${msg_file} NAME_WE)
|
||||
list(APPEND uorb_headers ${msg_out_path}/${msg}.h)
|
||||
list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp)
|
||||
list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h)
|
||||
endforeach()
|
||||
|
||||
if (px4_constrained_flash_build)
|
||||
@@ -300,6 +303,27 @@ add_custom_command(OUTPUT ${uorb_headers}
|
||||
)
|
||||
add_custom_target(uorb_headers DEPENDS ${uorb_headers})
|
||||
|
||||
add_custom_command(OUTPUT ${uorb_ucdr_headers}
|
||||
COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py
|
||||
--headers
|
||||
-f ${msg_files}
|
||||
-i ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
-o ${ucdr_out_path}
|
||||
-e templates/ucdr
|
||||
-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/ucdr_headers
|
||||
-q
|
||||
${added_arguments}
|
||||
DEPENDS
|
||||
${msg_files}
|
||||
templates/ucdr/msg.h.em
|
||||
tools/px_generate_uorb_topic_files.py
|
||||
tools/px_generate_uorb_topic_helper.py
|
||||
COMMENT "Generating uORB topic ucdr headers"
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
VERBATIM
|
||||
)
|
||||
add_custom_target(uorb_ucdr_headers DEPENDS ${uorb_ucdr_headers})
|
||||
|
||||
# Generate uORB sources
|
||||
add_custom_command(OUTPUT ${uorb_sources}
|
||||
COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py
|
||||
|
||||
@@ -0,0 +1,128 @@
|
||||
@###############################################
|
||||
@#
|
||||
@# EmPy template
|
||||
@#
|
||||
@###############################################
|
||||
@# generates CDR serialization & deserialization methods
|
||||
@#
|
||||
@# Context:
|
||||
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
|
||||
@# - search_path (str) Path to .msg files
|
||||
@###############################################
|
||||
@{
|
||||
import genmsg.msgs
|
||||
from px_generate_uorb_topic_helper import * # this is in Tools/
|
||||
|
||||
topic = spec.short_name
|
||||
uorb_struct = '%s_s'%spec.short_name
|
||||
|
||||
# get fields, struct size and paddings
|
||||
def add_fields(msg_fields, name_prefix='', offset=0):
|
||||
fields = []
|
||||
for field in msg_fields:
|
||||
if not field.is_header:
|
||||
field_size = sizeof_field_type(field)
|
||||
|
||||
type_name = field.type
|
||||
# detect embedded types
|
||||
sl_pos = type_name.find('/')
|
||||
if (sl_pos >= 0):
|
||||
package = type_name[:sl_pos]
|
||||
type_name = type_name[sl_pos + 1:]
|
||||
|
||||
# detect arrays
|
||||
a_pos = type_name.find('[')
|
||||
array_size = 1
|
||||
if (a_pos >= 0):
|
||||
# field is array
|
||||
array_size = int(type_name[a_pos+1:-1])
|
||||
type_name = type_name[:a_pos]
|
||||
|
||||
if sl_pos >= 0: # nested type
|
||||
|
||||
children_fields = get_children_fields(field.base_type, search_path)
|
||||
|
||||
for i in range(array_size):
|
||||
sub_name_prefix = name_prefix+field.name
|
||||
if array_size > 1:
|
||||
sub_name_prefix += '['+str(i)+']'
|
||||
sub_fields, offset = add_fields(children_fields, sub_name_prefix+'.', offset)
|
||||
fields.extend(sub_fields)
|
||||
else:
|
||||
assert field_size > 0
|
||||
|
||||
# note: the maximum alignment for XCDR is 8 and for XCDR2 it is 4
|
||||
padding = (field_size - (offset % field_size)) & (field_size - 1)
|
||||
|
||||
fields.append((type_name, name_prefix+field.name, field_size * array_size, padding))
|
||||
offset += array_size * field_size + padding
|
||||
return fields, offset
|
||||
|
||||
fields, struct_size = add_fields(spec.parsed_fields())
|
||||
|
||||
}@
|
||||
|
||||
// auto-generated file
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <ucdr/microcdr.h>
|
||||
#include <string.h>
|
||||
#include <uORB/topics/@(topic).h>
|
||||
|
||||
@##############################
|
||||
@# Includes for dependencies
|
||||
@##############################
|
||||
@{
|
||||
for field in spec.parsed_fields():
|
||||
if (not field.is_builtin):
|
||||
if not field.is_header:
|
||||
(package, name) = genmsg.names.package_resource_name(field.base_type)
|
||||
package = package or spec.package # convert '' to package
|
||||
print('#include <uORB/ucdr/%s.h>'%(name))
|
||||
}@
|
||||
|
||||
static inline constexpr int ucdr_topic_size_@(topic)()
|
||||
{
|
||||
return @(struct_size);
|
||||
}
|
||||
|
||||
bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf)
|
||||
{
|
||||
if (ucdr_buffer_remaining(&buf) < @(struct_size)) {
|
||||
return false;
|
||||
}
|
||||
@{
|
||||
for field_type, field_name, field_size, padding in fields:
|
||||
if padding > 0:
|
||||
print('\tbuf.iterator += {:}; // padding'.format(padding))
|
||||
print('\tbuf.offset += {:}; // padding'.format(padding))
|
||||
|
||||
print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size))
|
||||
print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name))
|
||||
print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
|
||||
print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
|
||||
|
||||
}@
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic)
|
||||
{
|
||||
if (ucdr_buffer_remaining(&buf) < @(struct_size)) {
|
||||
return false;
|
||||
}
|
||||
@{
|
||||
for field_type, field_name, field_size, padding in fields:
|
||||
if padding > 0:
|
||||
print('\tbuf.iterator += {:}; // padding'.format(padding))
|
||||
print('\tbuf.offset += {:}; // padding'.format(padding))
|
||||
|
||||
print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size))
|
||||
print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name))
|
||||
print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
|
||||
print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
|
||||
|
||||
}@
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,225 @@
|
||||
@###############################################
|
||||
@#
|
||||
@# EmPy template
|
||||
@#
|
||||
@###############################################
|
||||
@# Generates publications and subscriptions for XRCE
|
||||
@#
|
||||
@# Context:
|
||||
@# - msgs (List) list of all RTPS messages
|
||||
@# - multi_topics (List) list of all multi-topic names
|
||||
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
|
||||
@###############################################
|
||||
@{
|
||||
import os
|
||||
|
||||
import genmsg.msgs
|
||||
|
||||
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
|
||||
|
||||
topic_names = [s.short_name for s in spec]
|
||||
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
|
||||
send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
|
||||
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
|
||||
receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
|
||||
}@
|
||||
|
||||
|
||||
#include <uxr/client/client.h>
|
||||
#include <ucdr/microcdr.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
@[for idx, topic in enumerate(send_topics)]@
|
||||
#include <uORB/ucdr/@(send_base_types[idx]).h>
|
||||
@[end for]@
|
||||
@[for idx, topic in enumerate(recv_topics)]@
|
||||
#include <uORB/ucdr/@(receive_base_types[idx]).h>
|
||||
@[end for]@
|
||||
|
||||
// Subscribers for messages to send
|
||||
struct SendTopicsSubs {
|
||||
@[ for idx, topic in enumerate(send_topics)]@
|
||||
uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
|
||||
uxrObjectId @(topic)_data_writer;
|
||||
@[ end for]@
|
||||
|
||||
uxrSession* session;
|
||||
|
||||
uint32_t num_payload_sent{};
|
||||
|
||||
bool init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id);
|
||||
void update(uxrStreamId stream_id);
|
||||
};
|
||||
|
||||
bool SendTopicsSubs::init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id)
|
||||
{
|
||||
session = session_;
|
||||
|
||||
@[ for idx, topic in enumerate(send_topics)]@
|
||||
@{
|
||||
topic_pascal = topic.replace("_", " ").title().replace(" ", "")
|
||||
}@
|
||||
{
|
||||
|
||||
uxrObjectId topic_id = uxr_object_id(@(idx)+1, UXR_TOPIC_ID);
|
||||
const char* topic_xml = "<dds>"
|
||||
"<topic>"
|
||||
"<name>rt/fmu/out/@(topic_pascal)</name>"
|
||||
"<dataType>px4_msgs::msg::dds_::@(topic_pascal)_</dataType>"
|
||||
"</topic>"
|
||||
"</dds>";
|
||||
uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml,
|
||||
UXR_REPLACE);
|
||||
|
||||
uxrObjectId publisher_id = uxr_object_id(@(idx)+1, UXR_PUBLISHER_ID);
|
||||
const char* publisher_xml = "";
|
||||
uint16_t publisher_req = uxr_buffer_create_publisher_xml(session, stream_id, publisher_id, participant_id,
|
||||
publisher_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId datawriter_id = uxr_object_id(@(idx)+1, UXR_DATAWRITER_ID);
|
||||
@(topic)_data_writer = datawriter_id;
|
||||
const char* datawriter_xml = "<dds>"
|
||||
"<data_writer>"
|
||||
"<topic>"
|
||||
"<kind>NO_KEY</kind>"
|
||||
"<name>rt/fmu/out/@(topic_pascal)</name>"
|
||||
"<dataType>px4_msgs::msg::dds_::@(topic_pascal)_</dataType>"
|
||||
"</topic>"
|
||||
"</data_writer>"
|
||||
"</dds>";
|
||||
uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(session, stream_id, datawriter_id, publisher_id,
|
||||
datawriter_xml, UXR_REPLACE);
|
||||
|
||||
// Send create entities message and wait its status
|
||||
uint8_t status[3];
|
||||
uint16_t requests[3] = {
|
||||
topic_req, publisher_req, datawriter_req
|
||||
};
|
||||
if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) {
|
||||
PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", "@(topic_pascal)", status[0], status[1], status[2]);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@[ end for]@
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SendTopicsSubs::update(uxrStreamId stream_id)
|
||||
{
|
||||
@[ for idx, topic in enumerate(send_topics)]@
|
||||
{
|
||||
@(send_base_types[idx])_s data;
|
||||
|
||||
if (@(topic)_sub.update(&data)) {
|
||||
ucdrBuffer ub{};
|
||||
uint32_t topic_size = ucdr_topic_size_@(send_base_types[idx])();
|
||||
uxr_prepare_output_stream(session, stream_id, @(topic)_data_writer, &ub, topic_size);
|
||||
ucdr_serialize_@(send_base_types[idx])(data, ub);
|
||||
// TODO: fill up the MTU and then flush, which reduces the packet overhead
|
||||
uxr_flash_output_streams(session);
|
||||
num_payload_sent += topic_size;
|
||||
}
|
||||
}
|
||||
@[ end for]@
|
||||
|
||||
}
|
||||
|
||||
static void on_topic_update(uxrSession* session, uxrObjectId object_id,
|
||||
uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t
|
||||
length, void* args);
|
||||
|
||||
// Publishers for received messages
|
||||
struct RcvTopicsPubs {
|
||||
@[ for idx, topic in enumerate(recv_topics)]@
|
||||
uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))};
|
||||
@[ end for]@
|
||||
|
||||
uxrSession* session;
|
||||
|
||||
uint32_t num_payload_received{};
|
||||
|
||||
bool init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id);
|
||||
};
|
||||
|
||||
bool RcvTopicsPubs::init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id)
|
||||
{
|
||||
session = session_;
|
||||
uxr_set_topic_callback(session, on_topic_update, this);
|
||||
|
||||
|
||||
@[ for idx, topic in enumerate(recv_topics)]@
|
||||
@{
|
||||
topic_pascal = topic.replace("_", " ").title().replace(" ", "")
|
||||
}@
|
||||
{
|
||||
|
||||
uxrObjectId subscriber_id = uxr_object_id(@(idx)+1, UXR_SUBSCRIBER_ID);
|
||||
const char* subscriber_xml = "";
|
||||
uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(session, stream_id, subscriber_id, participant_id, subscriber_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId topic_id = uxr_object_id(1000+@(idx), UXR_TOPIC_ID);
|
||||
const char* topic_xml = "<dds>"
|
||||
"<topic>"
|
||||
"<name>rt/fmu/in/@(topic_pascal)</name>"
|
||||
"<dataType>px4_msgs::msg::dds_::@(topic_pascal)_</dataType>"
|
||||
"</topic>"
|
||||
"</dds>";
|
||||
uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, UXR_REPLACE);
|
||||
|
||||
uxrObjectId datareader_id = uxr_object_id(@(idx)+1, UXR_DATAREADER_ID);
|
||||
const char* datareader_xml = "<dds>"
|
||||
"<data_reader>"
|
||||
"<topic>"
|
||||
"<kind>NO_KEY</kind>"
|
||||
"<name>rt/fmu/in/@(topic_pascal)</name>"
|
||||
"<dataType>px4_msgs::msg::dds_::@(topic_pascal)_</dataType>"
|
||||
"</topic>"
|
||||
"</data_reader>"
|
||||
"</dds>";
|
||||
uint16_t datareader_req = uxr_buffer_create_datareader_xml(session, stream_id, datareader_id, subscriber_id, datareader_xml, UXR_REPLACE);
|
||||
|
||||
uint8_t status[3];
|
||||
uint16_t requests[3] = {topic_req, subscriber_req, datareader_req };
|
||||
if(!uxr_run_session_until_all_status(session, 1000, requests, status, 3))
|
||||
{
|
||||
PX4_ERR("create entities failed: %s %i %i %i", "@(topic)", status[0], status[1], status[2]);
|
||||
return false;
|
||||
}
|
||||
|
||||
uxrDeliveryControl delivery_control = {0};
|
||||
delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
|
||||
uxr_buffer_request_data(session, stream_id, datareader_id, input_stream, &delivery_control);
|
||||
|
||||
}
|
||||
|
||||
@[ end for]@
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void on_topic_update(uxrSession* session, uxrObjectId object_id,
|
||||
uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t
|
||||
length, void* args)
|
||||
{
|
||||
RcvTopicsPubs* pubs = (RcvTopicsPubs*)args;
|
||||
pubs->num_payload_received += length;
|
||||
|
||||
switch (object_id.id) {
|
||||
@[ for idx, topic in enumerate(recv_topics)]@
|
||||
case @(idx)+1: {
|
||||
@(receive_base_types[idx])_s topic;
|
||||
if (ucdr_deserialize_@(receive_base_types[idx])(*ub, topic)) {
|
||||
pubs->@(topic)_pub.publish(topic);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@[ end for]@
|
||||
|
||||
default:
|
||||
PX4_ERR("unknown object id: %i", object_id.id);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -428,6 +428,16 @@ def generate_client(out_dir):
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir,
|
||||
out_dir, uorb_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_CLIENT_TEMPL_FILE)
|
||||
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir,
|
||||
out_dir,
|
||||
uorb_templates_dir,
|
||||
package,
|
||||
px_generate_uorb_topic_files.INCL_DEFAULT,
|
||||
classifier.msg_list,
|
||||
fastrtps_version,
|
||||
ros2_distro,
|
||||
'dds_topics.h.em')
|
||||
|
||||
# Final steps to install client
|
||||
cp_wildcard(os.path.join(urtps_templates_dir,
|
||||
"microRTPS_transport.*"), out_dir)
|
||||
|
||||
@@ -529,7 +529,8 @@ if __name__ == "__main__":
|
||||
generate_output_from_file(
|
||||
generate_idx, f, args.temporarydir, args.package, args.templatedir, INCL_DEFAULT)
|
||||
|
||||
generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir)
|
||||
if os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])):
|
||||
generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir)
|
||||
copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet)
|
||||
elif args.dir is not None:
|
||||
convert_dir_save(
|
||||
|
||||
Reference in New Issue
Block a user