Daniel Mesham
3 years ago
committed by
Beat Küng
17 changed files with 1159 additions and 1 deletions
@ -1,3 +1,4 @@
@@ -1,3 +1,4 @@
|
||||
CONFIG_DRIVERS_HEATER=n |
||||
CONFIG_DRIVERS_OSD=n |
||||
CONFIG_MODULES_MICRORTPS_BRIDGE=y |
||||
CONFIG_MODULES_MICRODDS_CLIENT=y |
||||
|
||||
@ -1 +1,2 @@
@@ -1 +1,2 @@
|
||||
CONFIG_MODULES_MICRORTPS_BRIDGE=y |
||||
CONFIG_MODULES_MICRODDS_CLIENT=y |
||||
|
||||
@ -1 +1,2 @@
@@ -1 +1,2 @@
|
||||
CONFIG_MODULES_MICRORTPS_BRIDGE=y |
||||
CONFIG_MODULES_MICRODDS_CLIENT=y |
||||
|
||||
@ -0,0 +1,128 @@
@@ -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 @@
@@ -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; |
||||
} |
||||
} |
||||
@ -0,0 +1,119 @@
@@ -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) |
||||
@ -0,0 +1,5 @@
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_MICRODDS_CLIENT |
||||
bool "microdds_client" |
||||
default n |
||||
---help--- |
||||
Enable support for the MicroDDS Client |
||||
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
Subproject commit b5187a9f399e34cda7d2cdce8823295f83d9f3cc |
||||
@ -0,0 +1,544 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,91 @@
@@ -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}; |
||||
}; |
||||
|
||||
Loading…
Reference in new issue