dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

microdds: add xrce client

This commit is contained in:
Daniel Mesham 2022-02-11 17:11:34 +01:00 committed by Beat Küng
parent e080fab8f6
commit 06a9be74fa
17 changed files with 1159 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -64,3 +64,6 @@
path = src/lib/crypto/libtommath
url = https://github.com/PX4/libtommath.git
branch = px4
[submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"]
path = src/modules/microdds_client/Micro-XRCE-DDS-Client
url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git

1
Tools/astyle/files_to_check_code_style.sh
View File

@ -27,4 +27,5 @@ exec find boards msg src platforms test \
-path src/lib/crypto/monocypher -prune -o \
-path src/lib/crypto/libtomcrypt -prune -o \
-path src/lib/crypto/libtommath -prune -o \
-path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

1
Tools/kconfig/cmake_kconfig_lut.txt
View File

@ -139,6 +139,7 @@ mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y
mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y
micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y
microdds_client,CONFIG_MODULES_MICRODDS_CLIENT=y
navigator,CONFIG_MODULES_NAVIGATOR=y
px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y
rc_update,CONFIG_MODULES_RC_UPDATE=y

1
boards/px4/fmu-v5/rtps.px4board
View File

@ -1,3 +1,4 @@
CONFIG_DRIVERS_HEATER=n
CONFIG_DRIVERS_OSD=n
CONFIG_MODULES_MICRORTPS_BRIDGE=y
CONFIG_MODULES_MICRODDS_CLIENT=y

1
boards/px4/fmu-v5x/rtps.px4board
View File

@ -1 +1,2 @@
CONFIG_MODULES_MICRORTPS_BRIDGE=y
CONFIG_MODULES_MICRODDS_CLIENT=y

1
boards/px4/sitl/rtps.px4board
View File

@ -1 +1,2 @@
CONFIG_MODULES_MICRORTPS_BRIDGE=y
CONFIG_MODULES_MICRODDS_CLIENT=y

24
msg/CMakeLists.txt
View File

@ -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

128
msg/templates/ucdr/msg.h.em Normal file
View File

@ -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;
}

225
msg/templates/uorb_microcdr/dds_topics.h.em Normal file
View File

@ -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;
}
}

10
msg/tools/generate_microRTPS_bridge.py
View File

@ -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)

3
msg/tools/px_generate_uorb_topic_files.py
View File

@ -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(

119
src/modules/microdds_client/CMakeLists.txt Normal file
View File

@ -0,0 +1,119 @@
############################################################################
#
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client")
# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient
if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$")
set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE})
elseif(CMAKE_TOOLCHAIN_FILE)
set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake)
endif()
# Include NuttX headers
get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
set(c_flags_with_includes "${CMAKE_C_FLAGS}")
set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}")
foreach(dir ${include_dirs})
set(c_flags_with_includes "${c_flags_with_includes} -I${dir}")
set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}")
endforeach()
set(lib_dir ${CMAKE_INSTALL_LIBDIR})
if("${lib_dir}" STREQUAL "")
set(lib_dir "lib")
endif()
set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client)
set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR})
include(ExternalProject)
ExternalProject_Add(
libmicroxrceddsclient_project
PREFIX ${microxrceddsclient_build_dir}
SOURCE_DIR ${microxrceddsclient_src_dir}
INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR}
CMAKE_CACHE_ARGS
-DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER}
-DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER}
-DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}
-DCMAKE_C_FLAGS:STRING=${c_flags_with_includes}
-DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes}
-DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS}
-DUCLIENT_PIC:BOOL=OFF
-DUCLIENT_PROFILE_TCP:BOOL=OFF
-DUCLIENT_PROFILE_UDP:BOOL=ON
-DUCLIENT_PROFILE_SERIAL:BOOL=ON
-DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF
-DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=ON
-DUCLIENT_PLATFORM_POSIX:BOOL=ON
-DUCLIENT_BUILD_MICROCDR:BOOL=ON
-DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR}
-DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR}
-DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain}
BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
DEPENDS prebuild_targets git_micro_xrce_dds_client
)
add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL)
set_target_properties(libmicroxrceddsclient
PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
)
add_library(libmicrocdr STATIC IMPORTED GLOBAL)
set_target_properties(libmicrocdr
PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
)
add_library(microxrceddsclient INTERFACE)
add_dependencies(microxrceddsclient libmicroxrceddsclient)
add_dependencies(microxrceddsclient libmicrocdr)
add_dependencies(microxrceddsclient libmicroxrceddsclient_project)
target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include)
px4_add_module(
MODULE modules__microdds_client
MAIN microdds_client
STACK_MAIN 8000
SRCS
microdds_client.cpp
DEPENDS
microxrceddsclient
libmicroxrceddsclient
libmicrocdr
uorb_ucdr_headers
)
add_dependencies(modules__microdds_client topic_bridge_files)

5
src/modules/microdds_client/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig MODULES_MICRODDS_CLIENT
bool "microdds_client"
default n
---help---
Enable support for the MicroDDS Client

1
src/modules/microdds_client/Micro-XRCE-DDS-Client Submodule

@ -0,0 +1 @@
Subproject commit b5187a9f399e34cda7d2cdce8823295f83d9f3cc

544
src/modules/microdds_client/microdds_client.cpp Normal file
View File

@ -0,0 +1,544 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/cli.h>
#include <uORB/topics/vehicle_imu.h>
#include "microdds_client.h"
#include <uxr/client/client.h>
#include <uxr/client/util/ping.h>
#include <ucdr/microcdr.h>
#include <termios.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#define STREAM_HISTORY 4
#define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default
using namespace time_literals;
MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host,
const char *port, bool localhost_only)
: _localhost_only(localhost_only)
{
if (transport == Transport::Serial) {
int fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd < 0) {
PX4_ERR("open %s failed (%i)", device, errno);
}
_transport_serial = new uxrSerialTransport();
if (fd >= 0 && setBaudrate(fd, baudrate) == 0 && _transport_serial) {
if (uxr_init_serial_transport(_transport_serial, fd, 0, 1)) {
_comm = &_transport_serial->comm;
_fd = fd;
} else {
PX4_ERR("uxr_init_serial_transport failed");
}
}
} else if (transport == Transport::Udp) {
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
_transport_udp = new uxrUDPTransport();
if (_transport_udp) {
if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, host, port)) {
_comm = &_transport_udp->comm;
_fd = _transport_udp->platform.poll_fd.fd;
} else {
PX4_ERR("uxr_init_udp_transport failed");
}
}
#else
PX4_ERR("UDP not supported");
#endif
}
}
MicroddsClient::~MicroddsClient()
{
delete _subs;
delete _pubs;
if (_transport_serial) {
uxr_close_serial_transport(_transport_serial);
delete _transport_serial;
}
if (_transport_udp) {
uxr_close_udp_transport(_transport_udp);
delete _transport_udp;
}
}
void MicroddsClient::run()
{
if (!_comm) {
PX4_ERR("init failed");
return;
}
_subs = new SendTopicsSubs();
_pubs = new RcvTopicsPubs();
if (!_subs || !_pubs) {
PX4_ERR("alloc failed");
return;
}
int polling_topic_sub = orb_subscribe(ORB_ID(vehicle_imu));
while (!should_exit()) {
bool got_response = false;
while (!should_exit() && !got_response) {
// Sending ping without initing a XRCE session
got_response = uxr_ping_agent_attempts(_comm, 1000, 1);
}
if (!got_response) {
break;
}
// Session
uxrSession session;
uxr_init_session(&session, _comm, 0xAAAABBBB);
if (!uxr_create_session(&session)) {
PX4_ERR("uxr_create_session failed");
return;
}
// Streams
// Reliable for setup, afterwards best-effort to send the data (important: need to create all 4 streams)
uint8_t output_reliable_stream_buffer[BUFFER_SIZE] {};
uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE,
STREAM_HISTORY);
uint8_t output_data_stream_buffer[1024] {};
uxrStreamId data_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer,
sizeof(output_data_stream_buffer));
uint8_t input_reliable_stream_buffer[BUFFER_SIZE] {};
uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
uxrStreamId input_stream = uxr_create_input_best_effort_stream(&session);
// Create entities
uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
const char *participant_xml = _localhost_only ?
"<dds>"
"<profiles>"
"<transport_descriptors>"
"<transport_descriptor>"
"<transport_id>udp_localhost</transport_id>"
"<type>UDPv4</type>"
"<interfaceWhiteList><address>127.0.0.1</address></interfaceWhiteList>"
"</transport_descriptor>"
"</transport_descriptors>"
"</profiles>"
"<participant>"
"<rtps>"
"<name>default_xrce_participant</name>"
"<useBuiltinTransports>false</useBuiltinTransports>"
"<userTransports><transport_id>udp_localhost</transport_id></userTransports>"
"</rtps>"
"</participant>"
"</dds>"
:
"<dds>"
"<participant>"
"<rtps>"
"<name>default_xrce_participant</name>"
"</rtps>"
"</participant>"
"</dds>" ;
uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0,
participant_xml, UXR_REPLACE);
uint8_t request_status;
if (!uxr_run_session_until_all_status(&session, 1000, &participant_req, &request_status, 1)) {
PX4_ERR("create entities failed: participant: %i", request_status);
return;
}
if (!_subs->init(&session, reliable_out, participant_id)) {
PX4_ERR("subs init failed");
return;
}
if (!_pubs->init(&session, reliable_out, input_stream, participant_id)) {
PX4_ERR("pubs init failed");
return;
}
_connected = true;
hrt_abstime last_status_update = hrt_absolute_time();
hrt_abstime last_ping = hrt_absolute_time();
int num_pings_missed = 0;
bool had_ping_reply = false;
uint32_t last_num_payload_sent{};
uint32_t last_num_payload_received{};
bool error_printed = false;
hrt_abstime last_read = hrt_absolute_time();
while (!should_exit() && _connected) {
px4_pollfd_struct_t fds[1];
fds[0].fd = polling_topic_sub;
fds[0].events = POLLIN;
// we could poll on the uart/udp fd as well (on nuttx)
int pret = px4_poll(fds, 1, 20);
if (pret < 0) {
if (!error_printed) {
PX4_ERR("poll failed (%i)", pret);
error_printed = true;
}
} else if (pret != 0) {
if (fds[0].revents & POLLIN) {
vehicle_imu_s data;
orb_copy(ORB_ID(vehicle_imu), polling_topic_sub, &data);
}
}
_subs->update(data_out);
hrt_abstime read_start = hrt_absolute_time();
if (read_start - last_read > 5_ms) {
last_read = read_start;
// Read as long as there's data or until a timeout
pollfd fd_read;
fd_read.fd = _fd;
fd_read.events = POLLIN;
do {
uxr_run_session_timeout(&session, 0);
if (session.on_pong_flag == 1 /* PONG_IN_SESSION_STATUS */) { // Check for a ping response
had_ping_reply = true;
}
} while (poll(&fd_read, 1, 0) > 0 && hrt_absolute_time() - read_start < 2_ms);
}
hrt_abstime now = hrt_absolute_time();
if (now - last_status_update > 1_s) {
float dt = (now - last_status_update) / 1e6f;
_last_payload_tx_rate = (_subs->num_payload_sent - last_num_payload_sent) / dt;
_last_payload_rx_rate = (_pubs->num_payload_received - last_num_payload_received) / dt;
last_num_payload_sent = _subs->num_payload_sent;
last_num_payload_received = _pubs->num_payload_received;
last_status_update = now;
}
// Handle ping
if (now - last_ping > 500_ms) {
last_ping = now;
if (had_ping_reply) {
num_pings_missed = 0;
} else {
++num_pings_missed;
}
uxr_ping_agent_session(&session, 0, 1);
had_ping_reply = false;
}
if (num_pings_missed > 2) {
PX4_INFO("No ping response, disconnecting");
_connected = false;
}
}
uxr_delete_session_retries(&session, _connected ? 1 : 0);
_last_payload_tx_rate = 0;
_last_payload_tx_rate = 0;
}
orb_unsubscribe(polling_topic_sub);
}
int MicroddsClient::setBaudrate(int fd, unsigned baud)
{
int speed;
switch (baud) {
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
#ifndef B460800
#define B460800 460800
#endif
case 460800: speed = B460800; break;
#ifndef B921600
#define B921600 921600
#endif
case 921600: speed = B921600; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(fd, &uart_config);
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit, disable flow control */
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
return -1;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("ERR: %d (tcsetattr)", termios_state);
return -1;
}
return 0;
}
int microdds_client_main(int argc, char *argv[])
{
return MicroddsClient::main(argc, argv);
}
int MicroddsClient::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int MicroddsClient::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("microdds_client",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 4,
PX4_STACK_ADJUSTED(8000),
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
}
int MicroddsClient::print_status()
{
PX4_INFO("Running, %s", _connected ? "connected" : "disconnected");
PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
return 0;
}
MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
{
bool error_flag = false;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
Transport transport = Transport::Udp;
const char *device = nullptr;
const char *ip = "127.0.0.1";
int baudrate = 921600;
const char *port = "15555";
bool localhost_only = false;
while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 't':
if (!strcmp(myoptarg, "serial")) {
transport = Transport::Serial;
} else if (!strcmp(myoptarg, "udp")) {
transport = Transport::Udp;
} else {
PX4_ERR("unknown transport: %s", myoptarg);
error_flag = true;
}
break;
case 'd':
device = myoptarg;
break;
case 'b':
if (px4_get_parameter_value(myoptarg, baudrate) != 0) {
PX4_ERR("baudrate parsing failed");
error_flag = true;
}
break;
case 'h':
ip = myoptarg;
break;
case 'p':
port = myoptarg;
break;
case 'l':
localhost_only = true;
break;
case '?':
error_flag = true;
break;
default:
PX4_WARN("unrecognized flag");
error_flag = true;
break;
}
}
if (error_flag) {
return nullptr;
}
if (transport == Transport::Serial) {
if (!device) {
PX4_ERR("Missing device");
return nullptr;
}
}
return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only);
}
int MicroddsClient::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
MicroDDS Client used to communicate uORB topics with an Agent over serial or UDP.
### Examples
$ microdds_client start -t serial -d /dev/ttyS3 -b 921600
$ microdds_client start -t udp -h 127.0.0.1 -p 15555
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("microdds_client", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('t', "udp", "serial|udp", "Transport protocol", true);
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "serial device", true);
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:<param_name>)", true);
PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "<IP>", "Host IP", true);
PRINT_MODULE_USAGE_PARAM_INT('p', 15555, 0, 3000000, "Remote Port", true);
PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}

91
src/modules/microdds_client/microdds_client.h Normal file
View File

@ -0,0 +1,91 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/module.h>
#include <src/modules/micrortps_bridge/micrortps_client/dds_topics.h>
extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]);
class MicroddsClient : public ModuleBase<MicroddsClient>
{
public:
enum class Transport {
Serial,
Udp
};
MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port,
bool localhost_only);
~MicroddsClient();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static MicroddsClient *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
private:
int setBaudrate(int fd, unsigned baud);
const bool _localhost_only;
SendTopicsSubs *_subs{nullptr};
RcvTopicsPubs *_pubs{nullptr};
uxrSerialTransport *_transport_serial{nullptr};
uxrUDPTransport *_transport_udp{nullptr};
uxrCommunication *_comm{nullptr};
int _fd{-1};
int _last_payload_tx_rate{}; ///< in B/s
int _last_payload_rx_rate{}; ///< in B/s
bool _connected{false};
};

2
src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt
View File

@ -69,6 +69,7 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t
${micrortps_bridge_path}/micrortps_client/microRTPS_client.cpp
${micrortps_bridge_path}/micrortps_client/microRTPS_transport.cpp
${micrortps_bridge_path}/micrortps_client/microRTPS_transport.h
${micrortps_bridge_path}/micrortps_client/dds_topics.h
)
add_custom_command(OUTPUT ${topic_bridge_files_out}
@ -85,6 +86,7 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t
--idl-dir ${micrortps_bridge_path}/micrortps_agent/idl
>micrortps_bridge.log 2>&1 || cat micrortps_bridge.log
DEPENDS ${send_topic_files} ${receive_topic_files} ${PX4_SOURCE_DIR}/msg/tools/urtps_bridge_topics.yaml
${PX4_SOURCE_DIR}/msg/templates/uorb_microcdr/dds_topics.h.em
COMMENT "Generating RTPS topic bridge"
)
add_custom_target(topic_bridge_files DEPENDS ${topic_bridge_files_out})